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Research  into  nonlinear  estimation  techniques  for  terminal  homing  missiles  has 

been  conducted  for  many  decades.  The  terminal  state  estimator,  also  called  the  guidance 

filter,  is  responsible  for  providing  accurate  estimates  of  target  motion  for  use  in  guiding 

the  missile  to  a  collision  course  with  the  target.  Some  form  of  the  extended-Kalman  filter 

(EKE)  has  become  the  standard  estimation  technique  employed  in  most  modern  weapon 

guidance  systems.  EKF  linearization  of  nonlinear  dynamics  and/or  measurements  can 

cause  problems  of  divergence  when  confronted  by  highly  nonlinear  conditions.    The 

objective  of  this  dissertation  is  to  analyze  a  new  nonlinear  estimation  technique  that  is 

based  on  the  parameterization  of  the  nonlinearities.  This  parameterization  converts  the 

nonlinear  estimation  problem  into  the  form  of  a  steady-state  continuous  Kalman  filtering 

problem  with  state-dependent  coefficients. 


This  new  technique,  called  the  state-dependent  Ricatti  equation  filter  (SDREF), 
allows  the  nonlinearities  of  the  system  to  be  fully  incorporated  into  the  filter  design, 
before  stochastic  uncertainties  are  imposed,  without  the  need  for  linearization. 

The  SDREF  was  investigated  in  three  problems:  an  exoatmospheric,  terminal 
homing,  ballistic-missile  intercept  problem;  a  highly  nonlinear  pendulum  example;  and 
an  algorithmic  loss  of  observability  problem. 

The  exoatmospheric  guidance  problem  examined  nonlinear  measurements  with 
linear  dynamics.  To  investigate  the  SDREF  when  used  with  a  combination  of  nonlinear 
dynamics  and  nonlinear  measurements,  a  highly  nonlinear,  two-state  pendulum  problem 
was  also  examined. 

While  these  problems  were  useful  in  gaining  insight  into  the  performance 
characteristics  of  the  SDREF,  no  formal  proof  of  stability  could  be  determined  for  the 
original  formulation  of  the  estimator.  The  original  SDREF  solved  an  algebraic  SDRE 
that  arose  from  an  infinite-time  horizon  formulation  of  the  nonlinear  filtering  problem.  A 
modification  to  the  SDREF  formulation  was  developed  that  led  to  a  differential  SDREF, 
and  a  proof  for  local  asymptotic  stability  was  achieved.  The  modification  removed  the 
infinite-time  horizon  assumption  and  integrated  the  coupled  state-dependent  state  and 
covariance  equations.  This  new  form  of  the  estimator  is  called  the  modified  SDREF 
(MSDREF).  A  problem  involving  algorithmic  loss  of  observability  was  then  examined. 
This  problem  shows  a  performance  advantage  when  using  parameterization  versus 
linearization  as  in  the  EKF  technique. 
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CHAPTER  1 
INTRODUCTION 

The  extended  Kalman  filter  (EKF)  has  become  an  accepted  standard  for  both 
tactical  and  strategic  interceptor  guidance  problems.  While  some  problems  lend 
themselves  to  this  type  of  estimation  strategy,  systems  with  highly  nonlinear  dynamic 
environments  or  misunderstood  uncertainties  can  cause  divergence  of  the  filters.  Many 
alternative  nonlinear  estimation  techniques  have  been  researched  in  an  attempt  to 
overcome  this  problem.  The  linearization  of  the  nonlinear  dynamics  and/or 
measurements  is  one  area  of  concern.  The  EKF  linearizes  about  the  current  estimate  of 
the  state.  If  the  state  estimate  is  fairly  accurate,  this  method  has  been  shown  to  be 
effective.  If,  however,  the  estimates  are  poor,  linearization  can  cause  errors  in  the  filter, 
which  can  lead  to  degraded  performance  or  possible  divergence  of  the  state  estimate. 
It  is  this  linearization  that  motivates  the  following  research.  This  dissertation 
investigates  a  new  nonlinear  estimation  algorithm,  which  originates  from  a  recent 
nonlinear  regulator  design  technique.  It  uses  parameterization  to  bring  the  nonlinear 
system  to  a  linear-like  structure  with  state-dependent  coefficients  (SDC).    The  filtering 
equivalent  to  the  regulator  technique  can  be  realized  in  a  recursive  nonlinear  estimation 
algorithm,  which  has  the  structure  of  the  steady-state  continuous  Kalman  filter.  The 
Kalman  gain  is  found  by  solving  the  state-dependent  Ricatti  equation  at  each  filter  update 
time.  The  filtering  technique,  called  the  state-dependent  Ricatti  equation  filter  (SDREF), 
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allows  the  incorporation  of  nonlinear  dynamics  and/or  measurements  in  the  filter  design 

without  the  need  to  linearize  the  equations. 

The  SDREF  was  analyzed  in  a  several  ways  to  determine  the  benefits  and  issues 
associated  with  this  type  of  structure.  By  using  a  sampled-data  Bayesian  filter,  known  as 
the  bootstrap  filter,  the  "true"  probability  density  function  (pdf )  of  the  SDREF  states  was 
monitored.  This  allowed  an  investigation  into  the  differences  in  pdf  evolution  for 
linearizing  versus  parameterizing  filters.  Finding  the  true  pdf  can  be  difficult;  however 
recent  work,  by  the  author  on  the  bootstrap  estimator,  proved  valuable  for  this  analysis. 
The  bootstrap  technique  uses  thousands  of  samples  of  the  pdf  along  with  Bayes'  rule,  to 
estimate  the  true  pdf  of  any  given  state.  With  this  filter  and  a  large  number  of  samples, 
the  "true"  state  pdf  can  be  closely  approximated.  An  EKF  was  evaluated  in  a  similar 
manner  for  comparison  purposes. 

The  SDREF  was  exercised  in  three  problems.  A  realistic  and  detailed  evaluation 
of  the  filter  was  accomplished  by  examining  an  exoatmospheric  guidance  problem.  The 
SDREF,  bootstrap  filter,  and  EKF  were  coded  into  a  six-degree-of-freedom  (6-DOF) 
FORTRAN  simulation  for  analysis  in  an  exoatmospheric  terminal  homing  engagement 
using  passive  and  active  sensor  information.  The  sensor  measurements  included  azimuth, 
elevation  and  range.  The  characteristics  of  the  SDREF  and  EKF  were  evaluated  in 
several  different  engagement  scenarios.  The  SDREF  was  developed  in  a  Cartesian 
coordinate  frame,  which  causes  the  measurements  to  be  nonlinear,  while  the  dynamics 
remain  linear. 

The  second  problem  was  that  of  a  highly  nonlinear,  two-dimensional  pendulum 
problem.  Evaluation  of  the  SDREF  when  used  with  nonlinear  dynamics  and  nonlinear 
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measurements  was  examined.  The  SDREF  and  EKF  were  coded  into  the  visual 

simulation  environment  VisSim.  The  performance  of  the  filters  and  their  pdf's  were 

compared.  This  academic  problem  was  well  suited  to  exploring  any  differences  that 

nonlinear  dynamics  may  have  on  the  performance  or  pdf  characteristics  of  the  SDREF. 

No  formal  proof  of  stability  could  be  determined  using  the  above  formulations  of 
the  SDREF,  so  an  alternate  filter  formulation  was  sought.  The  original  SDREF  was 
based  on  an  infinite-time  horizon  formulation  of  the  nonlinear  filtering  problem  that 
resulted  in  an  algebraic  SDRE.  A  MSDREF  was  developed  that  utilizes  a  differential 
SDRE.  Because  the  Ricatti  equation  is  state  dependent,  it  must  be  integrated  in 
conjunction  with  the  state  estimates.  A  proof  of  local  asymptotic  stability  was  able  to  be 
determined.  The  new  MSDREF  was  examined  in  a  problem  consisting  of  loss  of 
algorithmic  observability;  and  the  results  indicated  a  significant  performance  advantage 
over  the  EKF. 

This  dissertation  fully  develops  the  requirements  for  the  SDREF  and  MSDREF.  It 
includes  an  investigation  of  controllability  and  observability,  which  are  point-wise  and  in 
the  linear  sense  and  pertain  to  a  valid  SDRE  solution.    An  exhaustive  investigation  of  the 
pdf  characteristics,  noise  sensitivity,  initial-error  robustness,  and  closed-loop  performance 
of  the  SDREF  using  the  EKF  as  a  comparative  source  was  performed.  A  theoretical  basis 
for  the  MSDREF,  when  used  as  an  observer,  was  shown.     The  following  research 
provides  theoretical  and  practical  information  demonstrating  that  the  SDREF  and 
MSDREF  are  viable  candidates  for  nonlinear  estimation,  and  for  some  problems  can  out- 
perform the  EKF. 


CHAPTER  2 
LITERATURE  SURVEY 

The  literature  abounds  with  research  of  various  nonlinear  estimation  techniques, 
most  of  it  mathematical  and  not  application  oriented.  An  elegant  presentation  of  the 
history  of  air-to-air  target  state  estimation  techniques  by  Cloutier  et  al.1  outlines  a  wide 
variety  of  techniques,  which  include  the  Kalman  filter,  EKF,  second-order  Gaussian  filter 
(SOGF),  recursive  nonlinear  filter,  recursive  maximum-likelihood  filter,  and  the  assumed 
density  filter.  These  filters  have  also  been  examined  in  the  context  of  single-  and 
multiple-model  adaptive  filters.    Adaptive  filters  may  make  use  of  certain  properties  of 
the  residual  or  covariance  to  continuously  change  filter  parameters  to  gain  better  filter 
performance.  On  the  other  hand,  multiple-model  filters  use  banks  of  filters  to  model 
many  possible  target  dynamic  situations.    Various  methods  are  then  used  to  either  select 
the  results  of  one  filter  or  combine  that  of  several. 

With  the  advent  of  the  Strategic  Defense  Initiative  (SDI),  a  new  set  of  problems 
arose  when  estimating  the  motion  of  a  boosted  intercontinental  ballistic  missile  (ICBM) 
with  the  use  of  passive-only  measurements.  While  the  EKF  is  still  the  most  widely 
accepted  estimator  for  this  type  of  engagement,  it  has  been  shown  to  have  degraded 
performance  when  stressed  by  highly  maneuverable  targets.2"4  This  is  the  same  problem 
that  tactical  filters  experience  in  maintaining  track  of  highly  maneuverable  aircraft  or 
missiles.  When  tracking  a  highly  nonlinear  maneuver,  state  estimates  can  become  poor 
and  the  linearization  assumption  begins  to  break  down.  The  specific  problems  associated 
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with  guidance  filters  for  the  exoatmospheric  intercept  problem  have  been  addressed  in 
several  research  programs  noted  in  the  following  paragraph. 

Schmidt  investigated  an  extension  of  work  by  Daum  in  which  a  closed-form 
solution  to  the  Fokker-Planck  equations  for  propagating  the  pdf  was  developed.  A  target- 
maneuver-detection  algorithm  was  added  to  this  formulation  to  enhance  performance. 
The  emphasis  in  this  research  was  to  more  accurately  model  the  target  acceleration  based 
on  the  rocket  equation,  instead  of  a  Markov-process.  The  target  model  caused  the  state 
equations  to  become  nonlinear  as  well.  The  results  of  the  work  did  show  an  improvement 
over  the  EKF  technique;  however  this  was  accomplished,  in  part,  through  the  use  of  the 
maneuver-detection  algorithm.  Gido3"4  developed  a  nonlinear  estimation  algorithm, 
which  numerically  implements  the  theoretical  work  of  Kolmogorov.  The  algorithm 
places  a  grid  over  the  pdf  and  uses  the  Fokker-Planck  equations  coupled  with  Bayes'  rule 
to  provide  the  state  estimates.  While  this  method  showed  improved  performance  over  the 
EKF,  it  is  computationally  intensive.  In  the  author's  masters'  thesis  at  the  University  of 
Florida,   another  novel  nonlinear  estimation  technique  was  investigated.  It  involved 
using  thousands  of  samples  of  the  pdf,  along  with  the  state  dynamics  and  Bayes'  rule  to 
propagate  and  update  the  pdf  ,6"7  The  technique  places  no  restrictions  on  the  type  of 
statistics  used  to  model  noise.  It  also  makes  no  linearization  assumptions.  While 
computationally  intensive,  it  is  directly  applicable  to  a  massively  parallel  processing 
architecture  since  the  same  operation  is  done  to  each  sample. 

While  the  bootstrap  method  may  not  be  seen  in  any  missile  guidance  system 
design  in  the  near  future,  it  does  provide  a  brute-force  method  for  obtaining  a  reasonable 
representation  of  the  true  state  PDF  for  a  given  problem.  It  is  in  this  context  that  the  filter 


6 
is  used  for  the  current  research.  While  many  of  the  above  filters  were  shown  to  have 
performance  superior  to  that  of  the  EKF,  realistic  implementation  may  be  difficult. 

Other  techniques  that  have  been  proposed  to  solve  the  bearings-only  guidance 
problem  include  the  modified  gain  pseudo-measurement  filter  (MGPMF)8  and  modified 
gain  EKF  (MGEKF).    Both  use  pseudo-measurements  to  avoid  the  need  for  linearization 
around  a  "nominal"  trajectory,  such  as  is  found  in  the  EKF.  These  methods  are  restricted 
to  a  special  class  of  nonlinear  functions,  which  can  be  manipulated  into  a  pseudo- 
measurement  form. 

The  construction  of  implementable  nonlinear  filters  for  nonlinear  stochastic 
systems  remains  a  challenge.  Numerous  filtering  algorithms,  based  upon  series 
expansions  to  approximately  realize  the  conditional  mean,  have  been  suggested  (see  the 
bibliography  of  Bucy  et  al.10  )  Once  again,  these  techniques  represented  a  heavy 
computational  burden,  even  for  low-order  dynamical  systems. 

The  SDREF  may  alleviate  some  of  the  problems  discussed  so  far.  The  technique 
is  derived  from  a  new  nonlinear  regulator  design  methodology  known  as  the  state- 
dependent  Ricatti  equation  (SDRE)  control  method.""12  The  objective  is  to  take  the 
nonlinear  system  described  by 

x  =  f(x)  +  g(x)u  (2-1) 

and  transform  it  to  the  state-dependent  coefficient  form 

x  =  A(x)x  +  B(x)u  (2-2) 

where  f(x)  =  A(x)x  B(x)  =  g(x)  (2-3) 
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using  direct  parameterization.  Once  in  this  form  the  SDREF  is  obtained  by  considering 
the  state-dependent  coefficients  to  be  fixed  and  applying  the  linear  continuous  Kalman 
filter  to  the  "frozen"  problem.  The  result  is  an  algebraic  SDRE  whose  structure  is  the 
same  as  the  steady-state  Ricatti  equation.  The  emphasis  of  this  research  is  to  develop  and 
analyze  a  guidance  filter  based  on  this  technique.  Initial  work  in  the  development  of  the 
SDREF    demonstrates  the  filter  in  a  simple  two-dimensional  pendulum  problem.  Other 
applications  include  induction  machine  estimation,14  simultaneous  state  and  parameter 
estimation,    and  gyro-less  satellite  angular  rate  estimation.16  The  current  research 
extends  their  work  to  include  an  exhaustive  investigation  into  the  internal  properties  of 
this  filter  in  a  strategic  interceptor  guidance  problem,  as  well  as  in  the  friction-pendulum 
problem.  Insight  into  the  internal  representation  of  the  pdf  when  using  parameterization 
versus  linearization  is  examined,  as  well  as  the  necessary  conditions  for  stability.  The 
first  example  presented  here  uses  only  nonlinear  measurements,  while  the  pendulum 
problem  discussed  herein  adds  the  complexity  of  nonlinear  dynamics  as  well.  A  third 
example  demonstrates  the  unique  parameterization  property  of  the  SDREF  when 
confronted  with  a  loss  of  algorithmic  observability. 


CHAPTER  3 
SCOPE  OF  WORK 

This  chapter  outlines  the  scope  of  the  work  in  this  dissertation.  Throughout  the 
design  and  analysis  of  the  SDREF,  the  EKF  was  used  to  compare  performance  issues 
between  linearized  and  parameterized  methods.  Similarly  a  sampled-data  bootstrap  filter 
was  used  to  compare  probability  distribution  functions  between  the  different  nonlinear 
filters  in  the  exoatmospheric  guidance  problem.  The  three  filters  are  theoretically  derived 
and  application-specific  equations  are  formulated  for  each  problem.  Controllability  and 
observability  requirements  for  the  SDREF  algorithm  are  analyzed  and  determined. 
Several  performance  measures  are  investigated  that  include  sensitivity  to  initial  errors, 
noise  parameters,  and  target  maneuvers.  In  addition,  a  somewhat  nonstandard 
methodology,  the  bootstrap  filter,  is  used  to  investigate  the  generally  unobservable 
evolution  of  the  pdf  within  the  filter.  The  pdf  was  examined  at  distinct  time  steps  to 
determine  whether  the  parameterized  filter  pdf  is  different  from  the  linearized  filter.  All 
three  filters  were  coded  in  FORTRAN  and  installed  in  a  6-DOF  simulation  for  evaluation 
in  an  exoatmospheric  terminal  homing  engagement. 

To  examine  the  effects  of  both  dynamic  and  measurement  nonlinearities,  the 
SDREF  and  EKF  were  also  coded  in  VisSim  (a  visual  simulation  tool  for  a  simple,  but 
highly  nonlinear,  pendulum  problem).  Performance  was  measured  in  a  manner  similar  to 
the  exoatmospheric  guidance  problem.  Because  no  formal  proof  for  stability  could  be 
found  for  the  SDREF,  modifications  were  made  that  allowed  a  detailed  proof  of  local 
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asymptotic  stability  to  be  found.  The  MSDREF  is  fully  derived  and  an  examination  of  its 

performance  against  the  EKF  for  a  problem  consisting  of  loss  of  algorithmic  observability 

is  also  explored. 


CHAPTER  4 
EXOATMOSPHERIC-GUIDANCE-PROBLEM  ENGAGEMENT  SCENARIO 

The  filters  were  initially  analyzed  in  the  context  of  an  exoatmospheric  terminal 
homing  engagement  of  an  interceptor  tracking  a  thrusting  ICBM  for  approximately  the 
last  10  seconds  before  impact.  The  interceptor  was  closing  on  the  target  at  approximately 
10  kilometers  per  second.  The  target  may  perform  thrust  cut-off  or  other  evasive 
maneuvers,  which  provide  for  a  stressing  nonlinear  engagement  scenario  in  which  to  test 
the  filters.  At  the  start  of  the  engagement,  the  guidance  filter  was  given  erroneous  initial- 
state  and  covariance  estimates.  The  sensor  is  assumed  to  have  acquired  the  target  and  the 
guidance  filter  is  ready  to  estimate  the  relative  motion  between  the  two  vehicles  using 
passive-only  measurements.  The  estimates  were  used  by  a  standard  augmented 
proportional-navigation  (PRO-NAV)  guidance  law  to  command  accelerations  that  keep 
the  interceptor  on  a  collision  course  with  the  target.  No  errors  associated  with  the  attitude 
control  system  were  used  as  this  would  complicate  the  analysis  and  cloud  the 
performance  comparison. 

Figure  1  shows  a  model  of  the  interceptor,  a  generic  4-kilogram  vehicle 
configured  with  divert  and  attitude  control  thrusters.  A  generic,  SS-18-like  ICBM  model 
was  used  to  generate  the  target  motion.    The  interceptor  may  have  many  error  sources, 
some  of  which  include  hand-over  errors,  pointing  errors,  thrust  errors,  center-of -gravity 
location  errors,  and  seeker  noise.  Initially  many  of  these  noise  sources  were  not  used  so 
that  the  filter  characteristics  could  be  more  easily  discerned.  Several  engagement  closing 
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angles  were  examined  to  allow  for  more  stressing  conditions  (i.e.,  tail  chase,  nose-on, 
beam  shot).  Once  proper  filter  operation  was  assured,  noise  sources  were  added  to  more 
realistically  emulate  the  true  intercept  scenario,  as  well  as  to  identify  weaknesses  in  filter 
performance. 


Divert  Thrusters 


Attitude  Control  Thrusters 


Z  Z 

Figure  1 .  Generic  intercept  vehicle. 


The  simulation  used  was  the  Guided  Simulation  Program  Exoatmospheric  (GSP- 
EXO).17  It  is  a  FORTRAN  simulation  developed  by  the  General  Electric  Company  for 
the  Air  Force.    Figure  2  shows  a  block  diagram  representation  of  the  subsystems 
modeled. 
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Figure  2.  Guidance  system  block  diagram 


To  allow  for  rapid  and  efficient  evaluation  of  filtering  concepts,  the  author  has 
modified  the  guidance  system.  It  has  a  Monte-Carlo  capability  with  an  outer  loop  around 
the  Monte-Carlo  loop,  to  allow  for  parameter  trades.  Analysis  was  performed  using  a 
personal  computer;  however,  analysis  can  be  accomplished  on  other  platforms,  including 
the  SUN,  VAX,  and  CRAY. 


CHAPTER  5 
EXOATMOSPHERIC-GUIDANCE-PROBLEM  FILTER  DEVELOPMENT 

The  following  paragraphs  outline  the  formulations  of  the  EKF,  bootstrap  filter, 
SDREF,  and  MSDREF  that  were  used  for  this  research.  A  brief  theoretical  description  of 
each  filter  is  given,  followed  by  the  application-specific  equations. 

There  are  three  filter  types  under  consideration  in  this  research  paper.  The 
mathematical  derivations  for  all  of  the  estimators  are  fully  developed  and  all  assumptions 
regarding  noise  characteristics  and  linearization  are  explained.  The  filters  are  presented 
in  coordination  with  the  problems  that  were  used  in  examining  their  performance.  The 
SDREF  theory  and  derivation  used  in  the  exoatmospheric  problem  is  shown  in  this 
Chapter,  the  SDREF  pendulum  problem  and  derivation  is  presented  in  Chapter  7,  while 
the  MSDREF  stability  proof  and  analysis  is  found  in  Chapter  8. 

Many  variations  of  SDREF  are  possible  due  to  the  infinite  number  of 
parameterizations  available.  Initially,  simple  parameterizations  were  used.  Another 
important  factor  in  evaluating  performance  differences  between  the  three  filters  was  the 
selection  of  the  noise  characteristics.  The  EKF  performance  may  be  enhanced  in  some 
cases  by  artistically  "tuning"  the  noise  parameters  within  the  filter.  To  assure  an  unbiased 
comparison  between  the  filters,  noise  parameters  were  set  to  the  same  nominal  values  for 
all  filters.  Due  to  several  limitations  on  the  bootstrap  filter,  developed  in  a  thesis  by  this 
author,  it  was  used  only  for  pdf  comparisons  in  the  analysis  of  the  strategic  guidance 
problem.  The  bootstrap  filter  needs  further  development  to  be  useful  in  stressing 
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engagements  as  the  number  of  samples  required  for  adequate  performance  makes  it 

intractable  to  run  for  large  numbers  of  comparison. 

The  SDREF  and  EKF  were  analyzed  in  great  detail  to  assess  key  performance 
characteristics  including  convergence,  stability,  and  state  estimation  error.  A  key 
performance  critique  was  the  comparison  of  the  evolution  of  the  SDREF  and  EKF  pdf  s 
to  that  obtained  by  using  the  bootstrap  filter.  Interesting  insight  into  the  differences 
between  how  the  SDREF  and  EKF  model  nonlinearities  was  observed. 

There  are  many  issues  involved  with  the  SDREF  development,  including  correct 
parameterization  of  the  nonlinear  measurement  equations,  investigation  into  the 
requirements  for  solution  of  the  SDREF,  and  the  stability  of  the  SDREF.  These  areas 
were  explored  to  determine  an  accurate  set  of  state  and  measurement  equations  and  a 
feasible  method  to  solve  them. 

An  initial  non  stressing  scenario  was  selected  to  establish  a  benign  comparative 
baseline  performance  assessment.  The  pdf's  were  compared  using  this  baseline  scenario 
as  well  as  a  nonlinear  maneuvering  target  scenario.  Once  the  baseline  was  established, 
the  filters  were  tested  against  more  stressing  engagements  and  intercept  angles. 
Performance  was  measured  by  examining  the  state  estimate  error  and  3-sigma  standard 
deviations,  as  well  as  comparing  the  state  estimate  to  the  true  state.  The  author  has  found 
that  this  is  an  accurate  method  of  evaluating  filter  performance. 

5.1  EKF  Derivation 

The  derivation  for  the  EKF  can  be  found  in  many  sources,18"19  and  is  presented 
here  for  completeness.  The  dynamics  for  the  intercept  problem  at  hand  are  linear,  while 
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the  measurement  equations  are  nonlinear.  For  this  reason  a  combination  of  linear  and 

EKF  equations  were  used  and  implemented  in  discrete  form.  Given  a  linear,  time-varying 

differential  equation  for  a  dynamic  system 

x(t)  =  F(t)x(t)  +  G(t)u(t)  +  L(t)w(t)  (5.1) 

where  x(0)  is  given,  u(t)  is  a  deterministic  input,  and  w(t)  is  a  random  disturbance,  the 
corresponding  discrete  time  solution  is 

£ 

*(f,)  =  *(»,.«M  )x(fM)+  ]«>(',, r)[G(r)M(T)  +  L(r)w(i-)]dr.  (5.2) 

If  u  and  w  are  assumed  piece-wise  constant  over  the  sampling  interval,  this  can  be 
written  as 

xk  =®m*m  +rMHj.,  +AMw»_,  (5.3) 

where  rt-i"t-i=  J«<f»,f)G(f)«d*  (5.4) 

and  At_,wt.,  =  J«(»t,T)  L(r)wdT.  (5.5) 

However  if  F ,  G,  and  L  are  also  considered  constant  over  the  sampling  interval,  AT,  then 
$(A0  =  em  (5.6) 

r(A()  =  O(Ar)[/„-O^l(A0]i;"lG  (5.7) 

A(A?)  a  <J>(A/)[/„  -<D"'(Af)]F-'Z,.  (5.8) 

Assuming  then  that 

£{;t„}=m„  .    E[(x-x0){x-xa)T)  =  Pii 

E{wt}  =  0  ,     £{wtw/}  =  e;        and 

E{wkwt_iT)  =0 
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the  state  propagation  equation  becomes 

*i  a*w*»-1+rMB*_1+AM(0)  (5.9) 

The  covariance  matrix  can  be  propagated  in  a  similar  manner  by 

H  =*t-.n-1®i-.r+0»-!  (5-10) 

with  flk-t-AnflUV,'.  (5.11) 

To  relate  the  covariance  matrix  of  random  sequences  to  the  spectral  density  of  the  random 
process  for  the  continuous  disturbance  input  w(t)  we  note  that 

E{w(t)wT(T)}  =  Q'c(t)S(t-T)  (5.12) 

The  equation  for  gt_,then  becomes 

&_,«  \<I>(tk,T)L(T)Q-c(T)LT(T)<!>T(tk,T)dT  (5.13) 

This  now  represents  the  integrated  effect  of  the  continuous  disturbance  input  w(l). 

Thus  the  two  equations  necessary  for  the  propagation  of  the  state  and  covariance 
are 

•c«=<I>J-,^-l  +  ri_,"t_1  (5.14) 

pt-*»-in-.®Mr+a-i  (5.15) 

where  Ot_,  is  defined  by  eF&l  and  rt_,  is  define  in  equation  (5.7). 

The  measurement  process  for  the  filter  is  now  determined.  Given  the  continuous, 
nonlinear  measurement  equation 

z(t)  =  h(x(t))  +  v(t).  (5.16) 

The  discrete  version  can  be  written  as 

hxK(xt{t))+vt  (5.17) 
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A  complete  derivation  of  the  EKF  update  equations  can  be  found  in,18  however  the  final 

solution  for  the  calculation  of  the  Kalman  gain,  and  the  update  of  the  covariance  and  state 


«i  =  ^(-^/(^-^(^(-^(-^/(^(-iH^r' 

Pt(+)  =  U  -  KkHt(it(-))]Pk(-)[I  -  KtHk(ik(-mr  +  KtRtKtT 
where  *,(-)  and  Pt(-)come  propagation  equations  (5.14)  and  (5.15)  and 


»*(*»(-)>  = 


Ac 


(5.18) 
(5.19) 
(5.20) 

(5.21) 


with  £{vt }  =  0  and  E{vkvkT)  =  Rk.  Now  (5.14)  and  (5.15)  and  (5.18)  through  (5.20) 

define  the  full  set  of  EKF  equations. 

For  the  exoatmospheric  intercept  problem  the  state  consists  of 


axT 
ayT 
az, 


(5.22) 


where  the  states  are  relative  position,  relative  velocity,  and  target  acceleration.  The 
system  equations  are 


vR  =  AT  -  Actm 
dT  =  -AaT  +  wT 


(5.23) 
(5.24) 
(5.25) 
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Putting  this  in  state  space  notation, 
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(5.26) 


From  this  equation  the  state  transition  matrix  is  computed  as 


0(Ar)  = 


/ 

lAt 

I(e-M'  +Mt-l)/A2' 

0 

I 

/(1-<TM')//1 

0 

0 

U-»* 

(5.27) 


where  /  is  the  3x3  identity  matrix. 

Assuming  the  commanded  acceleration  remains  constant  over  the  filtering  interval 

1 


\t>(tk,T)G{r)udT-- 


The  state  propagation  equation  becomes 


*t  =[*]**-,+ 


-ArA    7 

2 
-ArAc„m7 

0 


— Af2A    / 

2 
-ArA„m/ 

0 


(5.28) 


(5.29) 


The  covariance  propagation  proceeds  in  a  similar  fashion.  The  covariance  can  then  be 
propagated  by 

*i=[*KM  +  &-,  (5.30) 

The  measurement  equation  is 

z„=hk(Hrk)  +  vt  (5.31) 
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where  hk  (x(tk ))  = 


Range 

-zE  I  Range 
y„ I  Range 


Range  =  ^xR1  +  yR1+zR2  (5.32) 


and  vk=N(0,Rk)  (5.33) 

is  the  Gaussian  (normal),  random  vector  with  zero  mean,  and  covariance  Rt. 

This  completes  the  derivation  and  equation  formulation  for  the  nine-state  EKF 
needed  for  the  interceptor  guidance  system.  Given  an  initial  estimate  of  the  state  x(O) 
and  the  covariance  matrix  P(0)  equations  (5.14)  and  (5.15)  and  (5.18)  through  (5.20)  can 
be  solved  in  a  recursive  fashion  to  achieve  an  estimate  of  the  relative  vehicle  motion. 

5.2  Bootstrap  Estimator  Derivation 

The  bootstrap  filter  is  derived  using  the  recursive  Bayesian  estimation 
relationships.  To  begin  the  discrete  dynamic  state  equation  is  given  by 

**=/*-i  (*»-,.  w,-i)  (5.34) 

where  /,_,  is  the  system  transition  function  and  w,_,  is  a  random,  zero  mean,  white-noise 
sequence  representing  a  known  calculable  probability  distribution  function.  At  discrete 
times,  an  observation  becomes  available 

zk=hk(xk,vk)  (5.35) 

where  v,  is  a  zero  mean,  white-noise  sequence  representing  a  known  distribution 
function.  It  is  further  assumed  that  an  initial  pdf  p(x„\Y0)  of  the  state  vector  is  available. 
The  objective  of  this  process  is  to  construct  the  pdf  of  the  current  state  xk  given  all  the 
available  information;  i.e.,  p(xk\Yk). 
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The  prior  distribution  before  the  update  is  defined  as 

P(^1?w)-Jp(**'*1-i)P(*»-iI1,»-i)*c»-i  (5.36) 

where  the  probabilistic  model  of  the  state  evolution  can  be  obtained  through  the  use  of 
the  system  state  equations  and  the  known  statistics  of  wk  as 

P(xk\xk_l)  =  \p(xk\xk_l,wk_,)p(wk^\xk_])dwk_,  (5.37) 

Because  the  process  noise  is  assumed  to  be  independent  of  any  previous  state  values, 
p(wk_l\xl,_1)  =  p(wk_l).  If  xt_,  and  wt_,  are  known,  then  xt  can  be  arrived  at 
deterministically. 

The  update  stage  occurs  when  a  measurement  becomes  available.  The  a  priori 
density  may  be  updated  according  to  Bayes'  rule  as 

p(x^)  =  P(ytyP(xky 
p(yM-0 

where  the  normalizing  denominator  is 

P(y^yt-l)  =  jp(yt\xk)p(xk\Yk_l)dxt  (5.39) 

and  the  conditional  pdf,  p(yk\xk),  is  defined  by  the  measurement  model  and  the  known 
statistics  of  v, . 

The  method  by  which  the  following  theory  is  implemented  in  the  bootstrap  filter 
will  now  be  defined.  The  bootstrap  filtering  algorithm  provides  the  mechanism  for 
defining  a  set  of  random  samples  [xk_{(i)\  i  =  ],...,  N]  from  the  pdf  and  obtaining  the 
distribution  pix^Y^) .      The  propagation  stage  assumes  a  number  of  N  random 
samples  have  been  taken  from  some  initial  pdf  p(xk_l\Yk_l) .  Each  random  sample  for 
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each  state  is  then  passed  through  the  system  model  to  obtain  a  set  of  a  priori  samples  at 

time  step  k 

xt'(i)  =  ft(xk^(i),wk_](i)),i  =  l,...,N  (5.40) 

where  the  asterisk  represents  the  a  priori  sample. 

The  update  stage  of  the  filter  takes  each  of  the  a  priori  samples  and  computes  a  set 

of  a  posteriori  samples  from  which  the  estimate  can  be  obtained.  To  do  this,  the 

likelihood  of  each  a  priori  sample  l(xk  \yt)  is  calculated  using  the  measurement 

equations  and  a  normalized  weight  is  given  to  each  sample  by 

This  defines  a  discrete  distribution  over  the  a  priori  density.  The  normalized  values  of  q, 
can  be  resampled  using  a  uniform  distribution.  A  larger  value  of  q:  allows  more  samples 
from  that  bin  to  fall  into  the  a  posteriori  pdf.  The  state  associated  with  each  new  sample 

selected  is  assigned  to  the  new  a  posteriori  state  {xk(i)\  i  =  \ N}.  This  a  posteriori  set 

of  samples  is  approximately  distributed  as  p(xt\Yt).  From  this  the  mean  and  variance  for 
each  state  estimate  can  be  computed.  Because  this  filter  makes  no  assumption  of  linearity 
or  Gaussian  statistics,  it  was  useful  in  providing  a  close  approximation  of  the  true  pdf. 
This  semi-true  pdf  can  then  be  used  to  investigate  the  effect  of  linearization  and 
parameterization  on  the  evolution  of  the  pdf. 
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5.3  SDREF  Derivation 

The  SDREF  is  the  nonlinear  "dual"  of  a  nonlinear  regulator  control  design 
technique,  which  uses  a  parameterization  technique  to  bring  the  nonlinear  dynamic  and 
measurement  equations 

x  =  f(x)  (5.42) 

z  =  h(x)  (5.43) 

and  transform  them  into  a  linear-like  state-dependent-coefficient  (SDC)  form 

x  =  F(x)x  (5.44) 

z  =  H(x)x  (5.45) 

The  distinctive  assumption  in  the  SDREF  technique  is  that  the  SDCs  can  be 
treated  as  "frozen"  over  a  small  interval  to  give  adequate  piece-wise  performance.  The 
algebraic  Ricatti  equation  is  solved  at  every  filter  measurement  update  and  the  linear 
Kalman  filtering  algorithms  applied. 

The  technique  was  first  applied  to  the  following  nonlinear  regulator  problem.13 
The  task  is  to  minimize 

I=-jxTQ(x)x  +  uTR(x)udt  (5.46) 

with  respect  to  the  state  x  and  control  «,  subject  to  the  nonlinear  differential  constraint 

x  =  f{x)  +  g(x)u  (5.47) 

The  SDREF  approach  for  obtaining  a  sub  optimal  solution  is  to  use  direct 
parameterization  to  bring  the  nonlinear  dynamic  equation  to  the  SDC  form 

x  =  A(x)x  +  B(x)u  (5.48) 
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where  f(x)  =  A{x)x    and   B(x)  =  g(x) 

From  this  point  the  SDREF  can  be  solved  for  P  as 

AT(x)P+PA(x)-PB(x)R'(x)BT(x)P  +  Q(x)  =  0  (5.49) 

and  the  nonlinear  feedback  controller  can  be  constructed  as 

u  =  -R~'(x)BT(x)P(x)x  (5.50) 

The  SDREF  technique  is  shown13  to  have  desirable  properties  of  local  asymptotic  stability 

and  suboptimality  given  mild  conditions  of  stabilizability  and  detectability. 

It  also  shows  that  if  Ax(x)  and  A^x)  are  distinct  SDC  parameterizations,  they  can  be 

combined  to  yield  a  third  parameterization  by 

A(x,a)  =  aAl{x)  +  (l-a)A2(x)  (5.51) 

This  technique  of  combining  parameterizations  allows  an  infinite  number  of 

parameterization  possibilities  to  enhance  controller  design. 

An  example13  of  the  SDC  form  and  SDREF  is  repeated  to  introduce  the  method. 

Consider  the  stochastic  nonlinear  system 

x  =  f(x)  +  V(w)  (5.52) 

z  =  h{x)  +  v  (5.53) 

where  w  is  Gaussian  zero-mean  white  process  noise  with  E[w(t)wT(t  +  r)]  =  Q(t)S(t) 

and  v  is  Gaussian  zero-mean  white  measurement  noise  withE[v(t)vT(t  +  T)]  =  R(t)d(T). 

After  bringing  the  system  to  the  SDC  form 

x  =  F{x)x  +  Tw  (5.54) 

z  =  H(x)x  +  v  (5.55) 
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the  SDREF  is  given  by  the  steady  state,  linear,  continuous  Kalman  filter  equations  as 

x  =  F(x)x  +  K„(x)[z(x)-H(x)x]  (5.56) 

where  K  (x)  =  PHT(x)R~'  (5.57) 

and  P  is  the  positive  definite  solution  to  the  SDREF 

F(i)P  +  PFT(x)-PHT(x)R'H(x)P  +  rTQr  =  0  (5.58) 

The  preceding  method  was  applied  to  a  simple  two-dimensional  pendulum 

problem.13  Results  were  comparable  to  those  from  an  EKP.  The  main  thrust  of  the 

research  was  to  develop  a  SDREF  for  the  linear  dynamic  and  nonlinear  measurement 

equations  associated  with  the  strategic,  terminal  homing  guidance  problem  and 

investigate  filter  characteristics  using  the  EKF  and  bootstrap  filter  for  comparison  and 

analysis. 

5.3.1  Angle-Only  Measurement  SDREF 

The  following  derivations  are  performed  for  the  bearing-measurements  only 
SDREF.  It  shows  that  this  form  of  the  SDREF  is  not  detectable  and  therefore  violates  the 
necessary  conditions  to  assure  the  positive  definite  solution  of  the  Ricatti  equation.  Both 
the  continuous  and  discrete  forms  of  the  filter  are  given,  as  well  as  stabilizability  and 
detectability  analyses  for  each  type.  The  filter  derivation  begins  with  the  definition  of  the 
state  equations 

x  =  Fx  +  Gu+Lw    (Continuous  form)  (5.59) 

xk  =  <S>t^xt_,  +r,_lH1_l+A1_lw,_l     (Discrete  form)  (5.60) 


and  the  measurement  equations 


The  states  to  be  estimated  are 
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z  =  h(x)  +  v     (Nonlinear  form) 
z  =  Hx  +  v    (Linear  form) 


sxR 

% 

sz„ 

VXR 

vzR 
axT 

az 


■T  ) 

where  the  states  are  relative  position,  relative  velocity,  and  target  acceleration. 
The  system  equations  are 

vR  =  AT-  Anlm 
dT  =  —AaT  +  wT 
Putting  this  in  state  space  notation 


•5*1 

fo   / 

0  " 

M 

"0" 

0' 

M* 

0    0 

/ 

\'\ 

-/ 

aco 

„  + 

0 

dT\ 

0    0 

-AJ_ 

{":  \ 

0 

/ 

IF] 


u; 


\x}+  [G]    u  +  [L]w 


(5.61) 
(5.62) 


(5.63) 


(5.64) 
(5.65) 
(5.66) 

(5.67) 


The  measurement  equation  is 


zk  =h(x)  +  vt 


(5.68) 
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where,  based  on  the  small-angle  approximation, 


h(x)-- 


Range 


2    ,         2    .         2 


(5.69) 


Range  =  ^/A:fi2 
Range 

and  v^A'tO.fl,) 

The  SDREF  approach  for  obtaining  a  sub  optimal  solution  is  to  use  direct 
parameterization  to  bring  the  nonlinear  dynamic  equation  to  the  SDC  form 

x=F(x)x  +  G{x)u  (5.70) 

where  f(x)  =  F(x)x 

and  z  =  H(x)x  +  v  (5.71) 

From  this  point  the  SDREF  can  be  solved 

F(x)P  +  PFT(x)-PHT(x)R-'(x)H(x)P  +  LQLT(x)  =  0    (continuous)      (5.72) 
<&P<S>r-P-q>PHT(R  +  HPHTy'HPQ>T  +  Qt_l=0    (discrete).  (5.73) 

Since  the  system  dynamic  equations  are  linear,  the  only  parameterization  needed  is  for  the 
measurement  equation.  From  the  definition 

z  =  Hx  +  v  (5.74) 

the  linear  form  for  the  SDREF  can  be  cast  as 


H  = 


0      0       -\IR    0    0    0    0    0    0 
0    1/fi        0        000000 


(5.75) 


5.3.1.1  Continuous  SDREF 

Using  the  above-defined  state  and  measurement  equations  with  X  =  0.1,  the 
following  matrices  are  used  for  the  filter  equations 
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0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

-.1 

0 

0 

0 

0 

0 

0 

0 

0 

0 

-.1 

0 

0 

0 

0 

0 

0 

0 

0 

0 

-.1 

(5.76) 


The  time  constant  for  the  Markov  process  A,  was  based  on  an  expected  target 
acceleration  of  approximately  981  (m/s2). 


0 

0 

0" 

0 

0 

0 

0 

0 

0 

-1 

0 

0 

0 

-1 

0 

0 

0 

-1 

0 

0 

0 

0 

0 

0 

0 

0 

0 

(5.77) 


The  process  noise  used  for  this  problem  is  1  g  on  the  x-target  acceleration  and  3  g's  on 
both  y  and  z  target  accelerations.  The  disturbance  input  is 

E[w(t)]  =  0  (5.78) 

E[w(t),WT(T)  =  Qc(t)S(t-T)]  (5.79) 
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which  for  small  values  of  At  can  be  expressed  as 


Qi_l  =  LQ-cLTAt  = 


At 


(5.80) 


0000000  0  0 

0000000  0  0 

0000000  0  0 

0000000  0  0 

0000000  0  0 

0000000  0  0 

000000    96  0  0 

0000000  866  0 

0000000  0  866 


where  866  (m/s2)  is  the  variance  of  3  g's  of  acceleration  noise  in  the  y  and  z  directions. 
The  parameterized  measurement  matrix  can  be  computed  as 


H{x)-- 


0         0 

1 

0    0    0    0    0    0 

Range 

0         ' 
Range 

0 

0    0    0    0    0    0 

(5.81) 


The  standard  linear,  continuous  Kalman  filtering  equations  follow  as 

x  =  F(x)x  +  Kf  (i)[z(x)  -  H(x)x] 
where  Kf(x)  =  PHTRk~' 

and  P  is  the  positive-definite  solution  to  the  state-dependent  Ricatti  equation 

F(x)P+PFT(x)-PHT('x)RK^H(i)P  +  Qt_,=0 
where  £>,_,  =  LQC  'LTAt .  Ricatti  equation  theory  provides  for  a  legitimate  positive  semi 
definite  solution  if  the  following  conditions  are  met 

O', Q,PcR"\HT  e/?"",ft  e  R"m,Q  =  QT>0,R  =  Rr>0,m<n,xeQ  (5.85) 

and  (F,H)  is  point-wise  detectable  in  the  linear  sense  and(C,  F)  is  point-wise 


(5.82) 
(5.83) 

(5.84) 
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stabilizable  in  the  linear  sense  for  all  x  e  A  ,  where  C   is  a  full-rank  factorization  (FRF) 

of  Qt_i,  given  by 

(CTC)  =  Qt_l     and     rank(C)  =  rank((24_,)  (5.86) 

For  the  given  linear  dynamics  we  can  construct  the  controllability  matrix  as 

Con  =  [c    FC    ...F"-'C]  (5.87) 

If  Con  is  of  full  rank,  controllability  and  therefore  stabilizability  is  assured.  If  it  is  not 
fully  controllable,  a  minimum  condition  of  stabilizability  must  be  met.  Putting  the 
system  equations  into  the  controllability  canonical  form,  the  uncontrollable  poles  can  be 
checked  for  stability.20 

Using  a  similarity  transformation,  the  controllability  canonical  form  can  be  found 


F  =  TFT,    C  =  TC 

where  T  is  unitary  and  the  transformed  system  has  a  staircase  form 

F  = 


F,c 

0" 

,   c  = 

"0" 

fa 

Fc 

cc 

(5.1 


(5.89) 

where  (FC,GC)  are  controllable  and  (Fu.)  is  uncontrollable.  The  poles  of  the 
uncontrollable  subspaces  are  then  checked  to  insure  stability.  If  they  are  found  to  be 
stable,  then  stabilizability  is  verified. 

Using  the  noise  matrix  in  equation  (5.13)  and  performing  the  FRF  for  C,  it  can  be 
shown  that  the  controllability  matrix  has  full  rank  and  therefore  stabilizability  is  verified. 
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Similarly,  the  observability  Grammian  matrix  is  constructed  as 


Obs  = 


H 
HF 

HF"~' 


(5.90) 


If  Obs  is  of  full  rank  for  all  x,  then  full  point-wise  observability  in  then  linear  sense  is 
assumed.  Once  again  point-wise  detectability  needs  to  be  assumed  for  the  Ricatti 
equation  solution  to  be  positive  semi  definite.  If  it  is  not  of  full  rank  the  similar 
observability  canonical  form  can  be  used  to  again  check  the  point-wise  stability  of  the 
unobservable  poles.  This  canonical  form  is  given  by 


Fm    Ft, 

0      F„ 


H  =  [0    Ho] 


(5.91) 


Using  the  above  referenced  matrices  for  Fand  H,  it  can  be  shown  that  the  observability 
matrix  is  not  of  full  rank.  Therefore,  point-wise  stability  of  the  unobservable  poles  must 
be  checked.  It  was  found  that  the  poles  were  not  point-wise  stable  and  therefore  point- 
wise  detectability  was  not  verified.  A  similar  analysis  was  then  performed  on  the  discrete 
form  of  the  filter.  From  this  point  on,  only  the  discrete  form  of  the  SDREF  is  analyzed. 
This  form  was  chosen  due  to  the  use  of  discrete  measurements  that  are  only  available  at  a 
subset  of  the  integration  step  size  of  the  simulation.  The  discrete  filter  is  better  suited  for 
this  type  of  simulation  and  would  most  likely  be  used  in  any  realistic  guidance  software. 
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5.3.1.2  Discrete  SDREF 


The  discrete  form  of  the  SDREF  is  derived  using  the  following  formulas.  Since 
F  is  a  constant  matrix,  the  state  transition  matrix  can  be  found  using  Laplace  transforms 
as 


O(Af)  = 


/     IAt    I(e^+XAt-l)/£ 
0      /  I(\-e-M')/A 

0      0  Ie'** 


(5.92) 


When  using  X  =  0.1  and  At  =  0.025,  the  following  transition  matrix  is  computed 


<D  = 
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0 
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0 

.000312 
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0 

.000312 

0 

0 

0 
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0 
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0 
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0 

0 

0 

1 

0 

0 
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0 

0 

0 

0 

0 

0 

1 

0 

0 

.0249 

0 

0 

0 

0 

0 

0 

1 

0 

0 

.0249 

0 

0 

0 

0 

0 

0 

.9975 

0 

0 

0 

0 

0 

0 

0 

0 

0 

.9975 

0 

0 

0 

0 

0 

0 

0 

0 

0 

.9975 

(5.93) 


Assuming  the  commanded  acceleration  remains  constant  over  the  filtering  interval 


IV,  =j<t>(tt,T)G(T)udT-- 


2 
-AtAaJ 

0 


(5.94) 


The  state  propagation  equation  becomes 


=  [*&-,- 


2 
-Atl 

0 


(5.95) 


Also, 


and 
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E[wwT]  =  Q'cS(t-T)  (5.96) 

&_,  =  \®{tt,T)L(T)Q'c(T)LT(mT(tk,T)dT  (5.97) 

Using  1  g  of  acceleration  noise  on  the  x  target  acceleration  and  3  g's  in  v  and  z  and 
performing  the  above  integration,  the  process  noise  matrix  is  obtained. 


*k-\ 


9.46e-9           0  0 

0  8.54e-8  0 

0                0  8.54c- 

9.36e-7          0  0 

0  8.44e-6  0 


9.36£-7          0               0  4.99e-5  0               0 

0  8.44e-6          0  0  4.45e-4          0 

0                0  8.44«-6  0  0  4.45e-4 

9.98c-5         0              0  .006  0              0 

0  9.0e-4           0  0  .054             0 
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0 

8.44e-6 
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0 

9.0e-4 

0 

0 

.054 

4.98c-5 

0 

0 

.006 

0 

0 

.478 

0 

0 

0 

4.5e-4 

0 

0 

.054 

0 

0 

4.32 

0 

0 

0 

4.5<r-4 

0 

0 

.054 

0 

0 

4.32 

The  discrete  Ricatti  equation  solver  requires  the  following  conditions  to  be  met  to  insure 
that  a  legitimate  positive  semi  definite  solution  to  the  Ricatti  equation  is  obtained: 

<&T,Q,PeR"',HT  ERK"",ReRmxm,Q  =  QT  >0,R  =  RT  >0,m<n,xeCl     (5.98) 
and  (<S>,H)  is  point-wise  detectable  in  the  linear  sense,  and  (CO)  is  point-wise 
stabilizable  in  the  linear  sense  for  all  x  e  Q,  where  C  is  a  FRF  of{2,_,  and  is  defined  as 

(CTC)  =  Qt_{     and     rank(C)  =  rank(Q,_,)  (5.99) 

It  is  also  must  assumed  that  O  is  invertible  without  need  for  further  modifications  to  the 
solver.  If  all  assumptions  are  met,  then  the  Ricatti  equation  will  have  a  unique  non- 
negative  definite  solution  throughout  Q. 
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Using  the  above  referenced  matrices,  the  same  analysis  used  for  point-wise 

stabilizability  of  the  continuous  system  was  performed.  The  controllability  matrix  of  the 

pair  (C,<J>)  was  found  to  be  of  full  rank  and  therefore  the  system  is  stabilizable. 

Again  an  analysis  similar  to  the  continuous  version  was  performed  using  the  pair 
(O.H)  and  the  point-wise  observability  matrix  was  again  found  to  be  not  of  full  rank. 
The  canonical  form  was  calculated  and  again  the  poles  of  the  unobservable  subspace  were 
determined  to  be  point-wise  unstable  and  therefore  the  system  could  not  be  declared 
point-wise  detectable  in  the  linear  sense. 

Because  the  system  has  been  shown  to  be  point-wise  undetectable,  the  Ricatti 
equation  solver  cannot  provide  the  positive  semi  definite  solution  to  the  Ricatti  equation. 
Consequently,  another  approach  to  the  problem  must  be  examined.  One  way  to  make  the 
system  detectable  is  to  allow  a  measurement  in  the  range  or  ^-direction.  This  is  shown  in 
the  next  section. 

5.3.2  SDREF  With  Range  Measurement 

This  section  analyzes  the  same  filter  described  above  except  that  a  range 
measurement  has  been  added  to  the  measurement  equation.  By  adding  the  range 
measurement  the  system  becomes  both  stabilizable  and  point-wise  detectable  under  the 
conditions  set  forth  above.  Two  types  of  range  measurements  were  evaluated  to  provide 
adequate  observability  for  the  system. 
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5.3.2.1  Intensity  range  measurement 

The  range  measurement  used  is  the  source  radiant  intensity  of  the  target  arriving 
at  the  focal  plane  and  is 


Range2 


(5.100) 


where 


Ik  =  intensity     (watts) 

Jx  =  source  radiant  intensity 


watts 


i^sr  /jra) 
AA  =  bandwidth     (/mi) 
r  =  radius  of  seeker  aperature    (m) 
R  =  range  from  source  to  seeker    (m) 
vk  =  white  Gaussian  noise 

This  gives  the  measurement  equation: 


I-z / Range 
y  I  Range        \ 
J \AAxr2 1  Range2  J 

Putting  equation  (5.101)  into  the  SDC  form 


Range = ^ 


xr  +>«  +zr 


(5.101) 


H- 


0 

0 
J,AA7ir2 


-1 


0    0    0    0    0    0 


Range 
0 


Range 

0         0    0    0    0    0    0 


0 


0    0    0    0    0    0 


(5.102) 


Range  *xR 

The  measurement  noise  for  the  range  measurement  is  somewhat  more  complicated  as  it  is 
range  dependent.  The  actual  intensity  measurement  has  Gaussian  noise  from  the  focal 
plane  electronics,  which  can  cause  error  in  the  measurement.  However,  it  is  assumed  this 
error  is  small  in  comparison  to  the  fluctuations  of  the  source  radiant  intensity  and  is 


35 
assumed  to  be  zero.   Jx  is,  however,  comprised  of  both  the  deterministic  source  radiant 

intensity  and  a  random  part  given  by 

Ji  =  hd„+h„,  (5.103) 

The  resulting  intensity  contribution  from  the  random  fluctuations  in  the  plume 
core  can  be  calculated  from  equation  (5.101)  as 

7,     A  Am-1 


!,.,=- 


Range 

The  variance  of  this  random  component  of  intensity  is  obtained  by  taking  the  expected 
value  of  the  square  of  Irm  (i.e.,  E[IrJ } ).  The  equation  for  the  expectation  operation 
gives 

\(  Jt   AA.!rr2\\     (  M7ir 


(5.104) 


^    Range1    { 


yMj\.} 


I  Range 

If  P  ~  Ey2K„  }  >  then  the  range  dependent  measurement  variance  term  for  the  filter 
becomes 


(5.105) 


\  Range1 


(5.106) 


Using  appropriate  values  for  these  constants,  a  measurement  matrix  can  be  given  by 


2.5e-9         0  0 

0         2.5e-9         0 
0  0         8.9e-7 


(5.107) 


Using  the  above-determined  matrices,  the  SDREF  was  again  calculated.  The  filter 
responded  well  during  the  first  three  to  four  seconds  of  flight  and  then  diverged.  The 
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problem  lies  in  the  instability  of  the  Ricatti  equation  solution.  In  examining  the  point- 
wise  closed-loop  eigenvalues  of  the  system  (i.e.,  the  spectrum  of 

®T-HT(RK  +  HPHTy'HP<&  (5.108) 

with  x,  in  this  case  range,  frozen),  the  system  would  be  asymptotically  point-wise  stable 
in  the  linear  sense  if  the  closed-loop  eigenvalues  all  lie  inside  the  unit  circle.  The 
eigenvalues  of  the  SDREF  were  computed  for  each  filter  update  stage.  They  began  near 
the  unit  circle  (i.e.,  0.9992)  and  grew  to  become  unstable  as  the  simulation  ran.  The  final 
step  in  the  Ricatti  equation  solution  is  the  solution  of  an  «lh  order  linear  matrix  equation. 
A  number  was  computed  to  determine  the  condition  number  for  the  inverse  matrix.  It 
could  be  seen  that  the  condition  number  increased  (i.e.,  ill-conditioning)  as  the  closed- 
loop  eigenvalues  approached  the  value  of  one.  The  relatively  small  size  of  the  range 
variance  causes  R~'  to  become  very  large,  and  therefore  ill  conditioned.  Once  the  closed- 
loop  eigenvalues  became  larger  than  one,  the  problem  diverged  quickly.  Because  of  this 
numerical  instability,  a  different  range  measurement  was  tried. 

5.3.2.2  Distance  range  measurement 

The  new  range  measurement  is  the  distance  measurement  in  meters.  This  value  is 
assumed  to  be  available  from  the  signal  processing  algorithms.  The  new  measurement  is 

RanSe  =  4xK1  +  yR+lR  (5.109) 
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Therefore,  the  new  measurement  equation  becomes 


h(x)-- 


Range 
y 


Range 
Range 


(5.110) 


and  the  measurement  matrix  becomes 

0 
0 


H-- 


Range 


0 

1 
Range 

0 


-I 


0    0    0    0    0    0 


Range 

0         0    0    0    0    0    0 

0         0    0    0    0    0    0 


(5.111) 


Using  these  values  in  the  SDREF,  satisfactory  results  were  obtained.  (These  results  are 
given  in  Chapter  7.)  It  was  decided  that  this  would  be  the  formulation  that  would  be  used 
to  further  test  this  new  filtering  technique.  Before  these  results  are  presented,  however, 
an  investigation  into  the  theoretical  stability  of  the  SDREF  is  presented. 

5.3.3  Theoretical  Investigations 

One  of  the  main  theoretical  criteria  for  filtering  techniques  is  the  stability  of  the 
algorithm.  The  following  section  reviews  several  theoretical  stability  investigations. 
While  these  investigations  were  unsuccessful  in  proving  stability,  it  is  hoped  that  they 
provide  valuable  insight  into  the  SDREF.  It  is  these  attempts  that  led  to  the  development 
of  the  MSDREF  shown  Chapter  9.  While  the  methods  outlined  below  approach  the 
problem  from  several  different  theoretical  aspects,  all  were  unsuccessful  in  showing  local 
stability  for  the  SDREF.  Because  the  true  and  estimated  states  x  and  x  both  appear  in  the 
error  equation 
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e  =  (F(x)x  -  F(x)x  -  K(x)[H(x)x  -  H(x)x])  (5.112) 

each  of  the  approaches  fail  when  presented  with  the  cross  terms  that  are  created.  It  is 
hoped  that  this  section  will  prove  useful  to  future  stability  investigations.  The  approaches 
are  shown  starting  with  simple  linearization  techniques  and  progress  through  more 
complicated  and  sometimes  restrictive  theorems.  The  first  of  these  is  a  simple  Taylor 
series  expansion  of  the  parameterized  equations. 

5.3.3.1  Taylor  series  expansion 

The  following  stability  analysis  will  exploit  a  Taylor  series  expansion  about  the 
local  zero  point.  Consider  the  stochastic,  nonlinear  system 

x  =  f(x)  +  g(w)  (5.113) 

z  =  h(x)  +  v  (5.114) 

where  w  is  Gaussian  zero-mean  white  process  noise  with  E[w(t)wT(t  +  r)]  =  Q(t)S(T) 
and  v  is  Gaussian  zero-mean  white  measurement  noise  with  E[v(t)vr(t  + 1)]  =  R(t)S(z) . 
After  bringing  the  system  to  the  SDC  form 

x  =  F(x)x  +  Tw  (5.115) 

z  =  H(x)x  +  v  (5.116) 

the  SDREF  is  given  by  the  linear,  continuous  Kalman  filter  equations  having  state- 
dependent  coefficients 

x  =  F(x)x  +  K(x)[z(x)-H(x)x]  (5.117) 

where  K(x)  =  P(x)HT(x)R-'  (5.118) 

and  P  is  the  positive  semi  definite  solution  to  the  algebraic  SDRE 

Fix) P(x)  +  P(x)FT(x) -  P(x)HT(x)R-'H(x)P(x)  +  TTQT  =  0  (5.119) 
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Conjecture:  Assume  that  R  >  0,  Q  >  0,  /(*),  and  h(x)  e  RK ,  and  that  the 

parameterizations  F(x)  and  H( *)can  be  shown  to  satisfy  that  (F{x),H{x))is  a  point-wise 
detectable  pair  in  the  linear  sense  and  (C(x),  F(x))  is  a  point-wise  stabilizable  pair  in  the 
linear  sense,  where  CCT  =  Qt_t .  Also  assume  F(x)  e  C~  and  H(x)  e  C~  are  analytic, 
where  C"  is  the  class  of  functions  whose  derivatives  of  all  orders  are  continuous,  and  that 
/(0)  =  h(0)  =  0.  Then  the  SDREF  is  locally  asymptotically  stable. 
Outline  of  Attempted  Proof: 

For  ease  of  notation  the  proof  will  be  examined  in  scalar  form.  Given  that  the 
scalar  system  dynamic  equations  are 


i  =  F(x)x  +  Gu  +  Lw 
z-  H{x)x  +  v    and 

x  =  F(x)x  +  Gu  +  K[z-H(x)x] 


(5.120) 


and  defining  the  estimation  error  rate  as  e  =  x  -  x  the  above  equations  can  be  written  as 

£  =  (F  (x)x  -  F(x)x  -  K(x)[H(x)x  -  H(x)x])  (5.121) 

Now  expanding  h(x)  and  H(x)x  in  a  Taylor  series  expansion  about  zero 

h(x)  =  H{x)x  (5.122) 


h(0)i 


*(0) 


(*-©).. 


(-0/ 


*(0) 


Also  for  f(x)  =  F(x)x 


H(0)H 


f(x)  =  F(x)x 


(      \    3  " 


(,-o)2 


(5.123) 


/(0)1 


*(0) 


(-o) 


?S 


(*-oY 


f(0)+- 


a»<o) 


(J:-0)1 


(-o)2 


Similar  equations  can  be  written  for  h(x)  =  H(x)x  and  f(x)  =  F(x)x . 


Equating  like  terms 


and 


<*(0) 


etc 

df{0) 


dx 
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=  H(O) : 


=  F(0). 


*(0) 


dx 
df(0) 


Jx 


Using  this  and  recalling  that  K  -  PHT  R\ 

e(t)  =  (F(0)-  PHT(0)R-1H(0)))e(t)  +  H.O.T.  (5.124) 

Herein  lies  the  problem,  the  higher  order  terms  contain  both  x  and  x.  These  terms  cannot 
be  guaranteed  to  approach  zero,  much  less  go  to  zero  faster  than  the  first  term.  If  the 
error  dot  equation  could  be  expressed  in  this  linear  form  a  local  stability  proof  could  be 
achieved  in  the  following  manner.  For  ease  of  notation,  let  F(0)  =  F,  H(0)  =  H,  and 
s(t)  =  £.  Following  a  similar  derivation,19  a  Lyapunov  function  is  chosen 

V(£)  =  eTIe  (5.125) 

where  /  is  the  positive  definite  information  matrix,  the  inverse  of  P.  From  the  definition 
of  the  derivative  of  the  inverse, 

/  =  />-'=-//>/  (5.!  26) 

By  substituting  this  into  the  matrix  Riccati  equation,  a  differential  equation 

-i  =  IF  +  FTI  +  ILWLTI-HTN-iH  (5.127) 

is  yielded  that  has  the  steady-state  solution 

0=IF  +  FTI  +  ILWLTI-HTN-'H  (5.128) 
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The  time  derivative  of  the  Lyapunov  function  is 

V  =  2eTIe  =  2eT[F-KH]e 

=  2eTl[F-PHTN'H]e  (5.129) 

=  eT[(lF  -  HTN-[H)  +  (IF  -  HTN-'H)\e 

Using  (5.128)  this  can  be  written  as 

V  =  -^ILWLTl  +  HTN-lH]e  (5.130) 
As  the  term  in  square  brackets  is  positive  definite  at  all  times,  V  <  0  at  all  times.  This 
would  verify  the  local  asymptotic  stability  of  the  SDREF  for  some  small  neighborhood 
about  the  origin.  The  estimation  error  would  approach  zero  as  time  approaches  infinity. 

5.3.3.2  Using  a  Taylor  series  expansion  about  the  true  states 

This  approach  varies  slightly  from  the  above,  in  that  a  Taylor  series  expansion  of 
the  estimated  dynamics  and  measurement  equations  are  linearized  about  the  true  states. 
Using  the  same  state  and  measurement  equations  (5. 1 13-5. 120)  and  the  error  equation 
(5.121)  in  the  preceding  proof,  F(x)  and  H(x)  are  expanded  as 

F(x)  =  F(x)  +  F'(x)(x-x)  +  ^-^-(x-xf+...H.O.T.  (5.131) 

and       H(x)  =  H(x)  +  H'(x)(x-x)+ —(x-x)2+...H.O.T.  (5.132) 

Using  equations  (5.131)  and  (5.132)  equation  (5.121)  is  written  as 

e  =  {F(x)-KH{x)}e  +  {F'(x)-KH'(x)}ix±i}(e2)  (5.133) 

Once  again  the  problem  with  this  approach  can  be  seen.  The  only  way  it  can  be  shown 
that  the  second  term  disappears  or  is  negative  is  for  ex  to  be  zero.  This  can  only  be 
shown  to  be  true  for  the  linear  case. 
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5.3.3.3  Taylor  series  expansion  about  the  error 

Again,  using  the  same  system  equations  (5.1 13-5.121)  as  in  the  first  Taylor  series 
approach,  this  second  approach  linearizes  about  the  error.  Linearizing  the  dynamic  and 
measurement  equations 

F(x)  =  F(e)  +  F'(£)(x-e)+^^-(x-e)2+...H.O.T.  (5.134) 

F'(x)  =  F'(e)  +  F"(e)(x-e)+F    {E\x-ef+...H.O.T.  (5.135) 

H(x)  =  H(e)  +  H\e)(x-e)  +  ^-^-(x-e)2+...H.O.T.  (5.136) 


H'(x)  =  H'(e)  +  H"{e)(x-e)+H   (£\x-ef+...H.O.T.  (5.137) 

After  substituting  these  into  error  equation  (5.121), 

£  =  (F  -KH)£  +  (F'-KH')(x-£)£ 
+  {F'-KH')£x  +  (F"-KH")(x-£)£x 

+  {F"-KH"){x-E)2-  (5.138) 

2 

+  (F'"-KH'")(x-£)1-x  +  d(£2) 

Again,  the  error  equation  is  only  stable  if  it  can  be  shown  that  ex  -»  0.  This  can  only  be 
shown  to  be  true  for  the  linear  case. 


5.3.3.4  Residual  error  stability  approach 

The  following  approach  illustrates,  under  conditions  of  point-wise  detectability  in 
the  linear  sense  that  ensure  a  legitimate  solution  of  the  SDRE,  that  the  SDREF  can  be 
shown  to  be  asymptotically  stable.21  The  system  dynamics  and  filter  equations  are  given 
in  discrete  form  as 
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**«=****+«'.  (5.139) 

zt+,  =*Ut+1)  +  vt+1  (5.140) 

**♦!«=***  (5.141) 

**+i  =  W+X*+i  [zt+,-«t+,  (**««  )*t+./J  (5-142) 

*4+,  =  PM(^)HtJ(iM)RMl  (5.143) 

with  E[v1+,vt+,r]  =  Kt+1  (5.144) 

E[wlt,wt+,7']  =  e4+l  (5.145) 

where  i^,  (Jct+1 )  is  the  solution  of  the  SDRE.  These  are  the  equations  necessary  for  the 
discrete  SDREF.  It  is  also  assumed  that  vt+1  and  wt+1  are  random,  zero  mean  inputs  and 
therefore,  the  stability  of  the  SDREF  is  independent  of  these  error  sources. 

The  state  estimation  error  both  after  and  before  the  update  is  defined  respectively 
as 

**«"  *»♦!-■**+!  (5.146) 

Km*****-**!*  (5-147) 

Also,  for  ease  of  notation 

HM(xM)  =  HM  (5.148) 

^t,(*t+,)  =  -Pt+,  (5.149) 

**«=Zt+t-<Bi+i(z4+.)%n  (5.150) 

And  finally  a  candidate  Lyapunov  function  Vt+,  is  defined  as 

n+,  -s^/v1*™  (5.15D 

The  conditions  for  which  VM  is  a  decreasing  sequence  must  be  determined,  therefore 
showing  the  exponential  stability  of  the  SDREF. 
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In  the  preceding  stability  approaches,  linearization  of  the  filter  equations  has  not 

shown  stability  of  the  filter.  This  attempt  constrains  the  state  transition  matrix  and 

measurement  noise  matrix  so  as  to  characterize  the  error  due  to  linearization.  The 

measurement  residual  error  can  be  characterized  as 

eM=zM-Kxttlll)  (5.152) 

e*+i  =#*«(**«)**+,  ~ Ht+l{xM)xttl  (5.153) 

Rewriting  Hk+1(xk+l) using  a  Taylor  series  expansion,  for  each  output  error  prediction 

component  of  eM,  namely  eiM,  there  always  exists  a  residual  riMldue  to  the  first  order 

linearization  in  order  to  obtain  an  exact  equality.  Therefore, 

ei.t+i  =  Bt,M  (*m)*m  - {#a+i  (*i+i )  +  h')mi  0*m X***,  -  *»+,  )+■  ■  ■  }*t+,      (5. 1 54) 
and  the  residual  error  is 

'U+i  =  (tfa+,(xf+,)(ii+,-*(.+,)+...}*(+,  (5.155) 

Then  the  exact  measurement  error  can  be  defined  as 

*uw!X*ijm.|3»h/*+»!*h  (5.156) 

or  written  in  another  form 

h«*A*u*=«W^jw  (5.157) 

where  afJl+l  is  an  unknown  time-varying  scalar  related  to  >;  t+l  by 

^+i=(l-a,-,t+l)e,,i+i  (5.158) 

Introducing  the  signal  vector 

«j+i<Wi  =  BMSMt  (5.159) 

where  at+,  e  /?"  is  an  unknown  time  varying  diagonal  matrix  defined  as 

aktl=diag(aiM,...,apMI)  (5.160) 
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If  both  sides  of  equation  (5.142)  are  subtracted  from  xt+1, 

x^=^m-PMHk^RMXeM.  (5.161) 

Substituting  this  into  equation  (5.151),  and  using  the  fact  that  PM  =  PtJ 

**+l  —  *»+!/*     "t+l     Xk  +  Uk  ~  Xk-Hlk    "*  +  l    "t+1     e»+| 

t        _,  t  i  r  (5.162) 

Because  the  covariance  matrix  is  not  propagated  during  the  time  between  updates,  it  is 
assumed  that  the  a  priori  covariance  before  update  is  equal  to  the  previous  updated 
covariance, 

Pk,m=Pk  (5.163) 

Therefore, 

PM'  =  Pk~'  (5.164) 

Before  proceeding  further,  the  inverse  relationship  is  defined  as 

Pk*\      =  Pk+Uk     +Hk*l   RM    HM  (5.165) 

Once  again  a  similar  problem  arises.  Equation  (5. 165)  is  not  derivable  from  the  basic 
premise  of  the  SDREF  update  equations.21  If  this  equation  was  available,  it  could  be 
shown  that  Vk+l  is  a  decreasing  sequence.  Then 

**+l  =  *k+Uk  ~  Xk*\lk    "j+l    "*+l     ek*l  ,.  , ,-, 

t       -i  t       -l  r       -i  (5.166) 

~eM  rm  Hw*k+\ik+ek*\  rm   "MfMffM  Ri+1  eM 

and  reviewing  that 

#*+A+,  =«*+,  (5.167) 

and 


3cr*+iHri+i  =  eJ"i+ia7't+,  (5.168) 
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Equation  (5.166)  is  written 

as 

vM 

—  V                    T         TR      "' 

—  Nt+i/fc     ^+1  at+i  *H+i   fli+iei+ 

1 

—  e*+I    a*+]"*-t-i     "*+l**+V*  *"  **+ 

1    Rk» 

'"t+A+i 

(5.169) 

+  ekjRM'HMPMHMTRM 

■1 

And  using  the  fact  that 

^k+\!k~Xk    ***     "t+l/i     '"fc^* 

(5.170) 

with  Equations  (5.167-168),  Equation  (5.169)  is  rewritten 

vM 

-^^/K/^+r'aw-a* 

►A+l 

_si+i 

'at+. 

+A,iXWwV) 

et+. 

(5.170) 

+  x/{F/Pt-'F1-Pt-,}x( 

Therefore  under  the  condition  of  point-wise  detectability 

rpQlrirlirtnc  nn  /?  anH  D  (\  p      /?    >>  0  nnH  H    >  C\\  fliAn 

in  the  1 

inear  sense, 

and  certain 

vM-vt<o 

and  the  system  is  asymptotically  stable  as  the  values  in  parentheses  are  <  0. 

5.3.3.5  Lyapunov  approach  using  the  expectation  operator 

This  stability  approach  uses  a  similar  method.9  Given  the  system  equations 

xM  =  A,x,.  +  wj  (5.171) 

X,  =  xi  +  Ki[H{xl)xl-H(xl)x]  +  Klvl  (5.172) 
the  error  can  be  updated  and  propagated  by  the  equations 

et  m  x,  -  xj  (Updated  error)  (5. 1 73) 

ei  m  x,  -  x,  (Propagated  error)  (5. 1 74) 
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Using  equations  (5.171-5.172) 

e, -=x,  -x,-K[H(xl)xl-H(xi)x]-Klvl  (5.175) 

3  =  A-ie,-i  +  w,--i  (5.176) 

After  some  manipulation  of  these  equations,  the  error  update  equation  can  be  developed 

eM  =*A£l-AlKlH1xl  +  A,KJI^t-Klvl+wl  (5.177) 

where  //,  =  H(x)        and        H1  =  #(*) 

If  the  following  Lyapunov  function  is  defined 

VM-?MTeM  (5.178) 

it  can  be  shown  that 

EiVw(«w)-V;(«()}S0  (5.179) 

Substituting  equation  (5.177)  into  equation  (5.179),  the  following  equation  can  be 
computed 

K{VM(eM)-Vi(e,)}  =  eTiE{ATA-l}el 
+  E{2xATAKHlX-2xTATAKH,x} 
+  E{-2xTAT  AKH2x  +  2xT  AT  AKH2x] 
+  E{-2xTHlTKTATAKH2  x] 
+  E{xTHtTKTATAKH,x) 
+  E{xTH2TKTATAKH2x} 

In  this  equation  several  terms  exist  that  cannot  be  shown  to  be  negative  definite.  These 
terms  are  brought  about  by  the  same  problem  found  in  the  previous  approaches  evaluated 
to  show  stability.  They  are  cross  products  created  by  the  presence  of  both  .rand  x  in  the 
error  equation. 


(5.180) 


(5.182) 
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This  same  approach  was  also  tried  with  the  Lyupanov  function 

V  =  eTe  (5.181) 

and  Equation  (5.182)  was  developed. 

E{VM(eM)-V,(ei)}  =  eiTE{ATA-l}el 
+  E{2xATAKHlx  -  2xT  AT  AKH,x} 
+  E{-2xT  AT  AKH2x  +  2xTATAKH2x] 
+  E{-2xTHlTKTArAKH2x] 
+  E{xtHitKtKHix) 
+  e{xtH2tKtKH2x] 

Once  again  cross  terms  are  generated  and  cannot  be  shown  to  be  negative  definite.  All  of 
the  previous  attempts  at  stability  proofs  fail  for  the  same  reason:  The  cross  term  xx, 
produced  in  the  error  equation,  raises  significant  difficulties  in  finding  a  closed  form 
differential  equation  for  e .  Attempts  to  limit  the  problem  to  a  scalar  were  not  able  to 
resolve  the  cross  coupling  problem.  While  several  standard  and  non-standard  approaches 
were  tried,  a  satisfactory  stability  proof  could  not  be  verified.  It  is  hoped  that  these 
investigations  can  prove  useful  in  future  attempts  to  determine  filter  stability. 


CHAPTER  6 
EXOATMOSPHERIC-GUEDANCE-PROBLEM  SIMULATION  RESULTS 

Chapter  6  investigates  the  comparative  performance  of  each  filter  against  a  variety 
of  engagement  conditions  and  noise  parameters.  Before  any  study  on  filter  performance 
was  done,  a  baseline  performance  analysis  was  completed,  which  allowed  a  detailed 
investigation  of  the  SDREF  against  one  engagement  scenario.  Once  the  baseline 
performance  was  established,  further  investigation  into  filter  sensitivity,  robustness,  and 
closed-loop  performance  was  performed.  A  study  of  pdf  evolution  for  each  filter  is  also 
shown. 

6.1  Baseline  Performance 

To  establish  a  baseline  performance  measure,  an  engagement  representing  the 
final  10  seconds  of  a  closing  interceptor/boosting  ICBM  engagement  was  chosen.  Table 
1  details  the  major  initial  filter  parameters.  The  aspect  angle  is  defined  as  the  initial  Euler 
rotations  between  the  axes  of  the  interceptor  and  the  target.  For  example,  an  aspect  angle 
of  (30,30,30)  would  signify  that  the  interceptor's  initial  velocity  vector  has  been  rotated 
30  degrees  about  each  axis.  An  aspect  angle  of  (0,0,0)  would  be  a  head-on  engagement. 
The  (30,30,30)  aspect  angle  was  chosen  so  that  all  axes  of  the  filter  would  be  exercised  in 
the  engagement.  SIGP  and  SIGVP  are  the  standard  deviations  for  the  random 
initialization  errors  on  the  "true"  position  and  velocity  states  respectively.  When  SIGP 
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and  SIGVP  are  zero,  the  filter  has  been  initialized  with  the  "true"  initial-intercept 

conditions.  SIGXHAT  are  the  values  used  to  initialize  the  covariance  matrix  for  the  EKF 

and  the  bootstrap  filter.  XHAT  is  the  initial  state  given  to  the  filters.  The  measurement 

noise  for  the  x-direction  is  a  range-dependent  noise  that  is  given  as  a  percentage  of  the 

range  (i.e.,  as  the  interceptor  gets  closer  to  the  target  the  noise  on  the  range  measurement 

decreases).  All  filters  were  made  to  operate  with  the  same  initial  conditions  and  statistics 

so  that  comparisons  could  be  made. 

A  perfect  filter  was  used  to  guide  the  vehicle  to  impact,  while  the  EKF,  SDREF, 

and  bootstrap  filter  were  run  open  loop.  The  bootstrap  filter  was  run  with  10,000 

samples. 


Table  1.  Filter  simulation  parameters  baseline  conditions 


Parameters 

(units) 

X 

Y 

Z 

Aspect  Angle 

(degrees) 

30 

30 

30 

SIGP 

(m) 

0 

0 

0 

SIGVP 

(m/s) 

0 

0 

0 

SIGXHAT 

ar2 

(m2) 

1.0E5 

1.0E5 

1.0E5 

<T? 

(m2/s2) 

1.0E3 

1.0E3 

1.0E3 

<r? 

(m2/s4) 

1.0E2 

1.0E2 

1.0E2 

XHAT 

R 

(m) 

1.0E5 

10.0 

10.0 

V 

(m/s) 

-1.0E4 

-335.9 

223.9 

A 

(m/s2) 

55.2 

57.40 

-38.2 

SIGQ  (EKF) 

(m2/s4) 

196.0 

196.0 

196.0 

SIGQ  (SDREF) 

(m2/s4) 

1.0E6 

1.0E3 

1.0E3 

Meas  Noise 

(%,//m,//m) 

0.1 

50.0 

50.0 
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The  following  graphs  reflect  filter  performance.  Figures  3  through  5  show  the 

position-state  estimate  errors  with  +3-v/<7standard  deviations  for  each  filter.  Velocity  and 

acceleration  performance  was  similar  between  the  EKF  and  SDREF.  The  bootstrap 

acceleration  estimates  are  poor  due  to  an  inability  to  forward  measurement  information  to 

the  second  derivative  states.  The  problem  can  be  remedied  by  the  use  of  millions  of 

samples  or  other  ad-hoc  improvement  schemes;  however,  computer  limitations  and  the 

scope  of  this  dissertation  do  not  allow  investigation  at  this  time.  Figures  3  through  5  give 

initial  insight  into  the  relative  performance  of  each  of  the  filters  for  the  given  scenario. 

All  of  the  filters  adequately  estimate  the  state  to  the  precision  necessary  for  an  intercept  to 

occur.  With  state  estimate  errors  remaining  within  ±3(7  standard  deviations,  the  filters 

perform  well.  The  bootstrap  filter  does  show  a  tendency  to  keep  a  wider  variance  than 

the  other  two  filters.  However,  as  discussed  in  paragraph  6.2,  when  the  EKF  and  SDREF 

are  run  in  Monte  Carlo  fashion,  they  too  have  wider  variances.  The  results  shown  here 

are  for  one  run,  while  noticing  that  the  bootstrap  filter  is  inherently  Monte  Carlo,  even  for 

one  run.  The  performance  curves  in  Figures  3  through  5  verify  proper  filter  operation  and 

establish  a  base-line  performance  from  which  more  in-depth  analysis  proceeds. 

It  is  interesting  to  note  in  Figures  6  and  7  that  the  standard  deviations  for  the  EKF 

and  SDREF  are  approximately  identical  after  the  initial  transients  in  the  EKF  die  out. 

This  indicates  that  once  the  EKF  has  settled  and  the  estimates  are  being  computed 

accurately,  the  EKF  covariance  propagation  and  update  are  equivalent  to  the  algebraic 

Ricatti  solution  being  computed  in  the  SDREF.  This  relationship  is  only  shown  for  one 

state;  however,  similar  results  were  found  for  all  states. 
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Figure  3.  EKF  position  errors  with  ±3<7  standard  deviations. 
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6.2  PDF  Distribution  Comparison 

Using  the  baseline  scenario,  a  comparison  of  the  posterior  (after  the  measurement 
update)  probability  density  functions  was  investigated,  to  determine  whether 
parameterizing  versus  linearizing  the  nonlinearities  has  an  effect  on  the  shape  of  the  pdf. 
The  scenario  given  in  Table  1  was  run  once  and  the  state  estimate  and  standard  deviation 
were  computed  three  times  during  the  engagement.  Samples  were  taken  at  1,  5,  and  9 
seconds.  The  pdf's  for  the  EKF  and  SDREF  were  computed  using  a  Gaussian 
distribution  function  based  on  the  mean  and  standard  deviation  at  each  time  increment. 
The  resulting  curves,  Figures  8  through  9,  do  not  represent  the  "true"  pdf  as  nonlinear 
dynamics  are  involved.  However,  they  do  show  the  basis  of  the  theoretical  filter 
formulation.  ("True"  pdf  functions,  calculated  using  Monte  Carlo  simulations,  are 
discussed  in  following  paragraphs.) 

The  pdf  for  the  bootstrap  filter  was  generated  by  computing  a  histogram  of  1 ,000 
samples,  curve  fitting  a  function  using  a  cubic  spline,  and  normalizing  the  area  under  the 
curve.  Figures  8  through  9  show  the  one-run  posterior  pdf  functions  for  the  filters.  The 
first  time  increment  is  taken  1  second  into  the  flight.  The  filters  are  still  trying  to 
converge  and  therefore  the  distributions  are  large.  This  is  demonstrated  in  Figure  8  by 
comparing  the  narrow  spike  of  the  highly  converged  bootstrap  filter  to  the  wide,  non- 
converged  pdfs  for  the  other  filters.  If  consideration  is  given  to  the  scaling  on  the  *-axis, 
no  concern  should  be  given  to  the  fact  that  the  distributions  are  not  near  the  true  state.  It 
can  be  seen  that  the  estimates  are  close  to  the  "truth".  The  ^-position  state  shows  a  more 
accurate  representation  of  the  pdf  than  the  v-  and  z-position  states.  This  is  due,  in  part,  to 
measurement  noise  decreasing  as  the  engagement  progresses,  and  a  more  accurate 
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knowledge  of  the  true  state  is  obtained.  Another  interesting  factor  shown  in  Figure  9,  is 

the  compression  of  the  EKF  and  SDREF  distributions.  This  is  typical  of  the  "falling 

asleep"  problem  associated  with  many  filters.  As  the  state  estimation  error  becomes 

small,  the  covariance  matrix  also  becomes  small,  and  the  filter  reacts  slowly  to  new  data, 

which  takes  the  estimate  away  from  the  converged  answer.  The  bootstrap  filter,  however, 

keeps  the  distribution  open  wider.    Even  using  the  one-run  pdf's,  the  EKF  and  SDREF 

pdf  shapes  appear  to  be  similar. 

To  determine  the  "true"  pdf  functions  for  the  EKF  and  SDREF,  the  filters  were 
run  in  a  100-run  Monte  Carlo  experiment.  Filter  state  estimates  were  saved  at  the  same 
time  increments.  The  samples  from  each  of  the  runs  were  used  to  generate  the  pdf  curves 
in  Figures  10  through  1 1.  The  same  histogram  method  described  in  the  previous 
paragraph  for  the  bootstrap  filter  was  used  to  plot  the  curves.  (The  bootstrap  filter  did  not 
need  to  be  examined  in  this  fashion,  as  its  formulation  is  inherently  Monte  Carlo.)  The 
largest  differences  can  be  seen  in  Figure  1 1 .  In  the  later  time  samples  (5  and  9  seconds), 
the  previously  tight  distributions  have  flattened  out,  while  the  wide  distributions  have 
tightened  up.  This  is  to  be  expected,  as  the  one-run  case  is  only  one  realization  of  the 
Monte  Carlo  runs. 

Summarizing  the  above  analysis  shows  that  the  EKF  and  SDREF  pdf's  do  not 
resemble  the  "true"  pdf.  However,  they  are  similar  to  each  other.  This  may  be  explained 
by  the  fact  that  the  EKF  linearization  method  provides  an  accurate  filtering  methodology 
as  long  as  the  filter  state  estimate  is  close  to  that  of  the  "true"  state.  If  a  target  maneuver 
is  introduced  that  causes  the  state  estimate  to  be  far  from  the  "truth",  then  it  is  assumed 
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Figure  10.  Monte  Carlo  posterior  probability  density  functions  at 
time  =1,5,  and  9  seconds  for  X  position. 
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that  the  distributions  between  the  EKF  and  SDREF  differ  due  to  the  different  theories  of 

linearization  versus  parameterization.  The  following  paragraphs  explore  this  scenario. 

The  filters  were  once  again  run  in  a  Monte-Carlo  simulation.  However,  this  time 
the  target  performs  a  thrust  cut-off  maneuver,  2  seconds  after  launch,  which  causes  the 
target  acceleration,  in  all  axes,  to  drop  to  zero  for  the  rest  of  the  flight.  This  is 
representative  of  timing  the  engagement  too  close  to  booster  burnout.  The  filters  are  still 
operating  under  the  assumption  of  a  Gauss-Markov  target  acceleration  model,  even 
though  no  target  acceleration  is  present  after  2  seconds.  The  same  initial  conditions  listed 
in  Table  1  were  repeated. 

Figures  12  through  14  show  the  divergence  from  the  "true"  acceleration-state  for 
the  filters.  For  previously  discussed  reasons,  Figure  14  shows  the  bootstrap  filter 
performs  poorly  when  tracking  the  acceleration  states.  The  deviation  from  the  "true" 
state  is  significant  after  the  maneuver  occurs.  This  is  a  good  test  to  determine  the 
differences  between  linearization  and  parameterization.  Once  again  the  Monte  Carlo  pdf 
functions  were  plotted  in  Figures  15  through  16  Samples  were  taken  at  2,  2.5,  and  3 
seconds  into  the  engagement.  This  allows  the  pdf  to  be  examined  before  and  during  the 
most  stressful  time  of  the  maneuver.  Suprisingly,  the  EKF  and  SDREF  pdf  shapes  do  not 
differ  significantly.  It  can  be  seen  in  Figure  15  that  although  the  "true"  probability  has 
become  bimodal,  the  shapes  are  still  similar.  While  the  single-run  EKF  and  SDREF 
distributions  can  not  be  bimodal,  Figures  15  through  16  represent  Monte  Carlo  results. 

The  results  from  the  pdf  comparison  experiments  indicate  that  there  is  no 
appreciable  difference  in  pdf  shapes  between  the  EKF  and  SDREF.  The  conclusion  may 
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Figure  12.  EKF  estimated  and  true  acceleration. 
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Figure  13.  SDREF  estimated  and  true  acceleration. 
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Figure  14.  Bootstrap  estimated  and  true  acceleration. 
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Figure  15.  Monte  Carlo  posterior  probability  density  functions  at 
time  =  2,  2.5,  and  3  seconds  for  thrust  cut-off  X  position. 
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be  drawn  that  the  two  filters  operate  with  identical  statistical  models  although  the  filters' 

method  of  handling  and  operating  on  the  stochastic  processes  are  significantly  different 

6.3  Sensitivity  Analysis 

The  following  section  examines  the  SDREF  sensitivity  to  measurement  noise, 
initialization  error,  and  the  robustness  properties  in  the  presence  of  stressing  target 
maneuvers.  Only  the  EKF  and  SDREF  are  used  for  this  area  of  the  study.  Further 
bootstrap  filter  development  is  needed  to  make  it  useful  in  less  than  ideal  conditions.  The 
EKF  and  SDREF  were  tuned  separately  to  give  them  the  best  chance  of  performing  well 
under  these  varying  conditions.  Therefore  each  filter  may  have  been  started  with  different 
initial  conditions  and  statistical  information.  The  following  studies  are  accomplished 
using  perfect  state  information  to  guide  the  vehicle,  while  the  EKF  and  SDREF  run  open 
loop.  Allowing  the  guidance  algorithms  access  to  perfect  state  information  removes  the 
closed-loop  guidance  algorithm  effects  from  the  analysis  of  filter  performance. 

6.3.1  Initialization  Error 

To  test  the  response  to  initialization  errors,  the  filters  were  given  erroneous  initial- 
state  estimate  errors  of  1  and  5  percent.  The  EKF  initial  covariance  was  set  to 
approximate  this  error.  The  SDREF,  however,  has  no  direct  means  of  initializing  the 
covariance  matrix  as  it  is  solving  for  the  covariance  matrix  at  each  filter  update  time. 
This  will  be  shown  to  be  a  detriment  for  this  particular  implementation  of  the  SDREF. 

Table  2  gives  the  initial  filter  conditions  for  the  EKF  and  1  percent  initial  error. 
Measurement  noise  was  held  constant  for  both  cases,  as  was  the  measurement  noise 
matrix  given  to  both  filters.     Range  measurement  noise  is  again  range  dependent  and  is 
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given  as  a  percentage  of  "true"  range.  The  initial  setup  for  the  SDREF  used  the  same 

parameters  as  those  of  the  EKF  except  that  higher  values  of  process  noise  were  tried  to 

increase  the  covariance  response.  The  only  tuning  parameters  in  the  SDREF  are  Qt_r  R, 

and  the  nonlinear  parameterization  methodology.  The  measurement  noise  matrix,/?,  is 

not  directly  related  to  the  initialization  error;  therefore,  only  the  other  two  tuning 

parameters  can  be  used.  Because  this  implementation  of  the  SDREF  has  linear  dynamic 

equations  the  only  method  available  for  tuning  is  to  adjust  Q,_, .  This  is  an  unacceptable 

way  of  tuning  the  filter  for  initialization  errors  as  it  causes  the  covariance  estimate  to 

remain  artificially  high  for  the  entire  flight.  The  SDREF  as  previously  derived,  does  not 

permit  any  initialization  of  the  covariance  matrix,  as  it  is  simply  calculated  from  the 

solution  of  the  SDRE.  This  deficiency  is  examined  later  in  this  section. 


Table  2.  EKF  and  SDREF  parameters  one-percent  initialization  error 


Parameters 

(units) 

X 

Y 

Z 

Aspect  Angle 

(degrees) 

30 

30 

30 

SIGP 

(m) 

1000 

35 

30 

SIGVP 

(m/s) 

100 

3.4 

2.2 

SIGXHAT 

ar2 

(m2) 

1.0E6 

1.0E3 

1.0E3 

ov2 

(m2/s2) 

1.0E4 

50.0 

50.0 

*/ 

(m2/s4) 

1.0E1 

I.0E1 

1.0E1 

XHAT 

R 

(m) 

1 .0E5 

10.0 

10.0 

V 

(m/s) 

-1.0E4 

-335.9 

223.9 

A 

(mis1) 

55.2 

57.4 

-38.3 

SIGQ  (EKF) 

(m2/s4) 

196.0 

196.0 

196.0 

SIGQ  (SDREF) 

(m2/s4) 

1.0E6 

1.0E3 

1.0E3 

Meas  Noise 

(%,jum,jum) 

0.1 

50.0 

50.0 
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Figures  17  and  18  show  the  position-state  response  of  the  EKF  and  SDREF  to  a  1- 

percent  initial  error.  (Note  the  oscillations  in  the  SDREF  estimate  during  the  initial 

settling  portion  of  the  flight.)  Velocity  and  acceleration  trends  were  similar. 

The  filters  were  then  run  with  a  5-percent  initial  error.  (The  initialization 
parameters  for  the  EKF  and  SDREF  are  given  in  Table  3.)  Because  of  the  type  of 
engagement  and  large  distances  and  closing  velocities  involved,  a  fixed-percentage  type 
of  initial  error  induces  large  disturbances  in  the  jt-direction.  Therefore,  little  attention  is 
given  to  the  performance  in  this  direction,  as  these  magnitudes  of  error  are  not  likely  to 
occur.  Figures  19  through  20  show  the  performance  for  a  5-percent  initial  error.  The 
inability  to  properly  adjust  the  initial  covariance  matrix  in  the  SDREF  causes  a  large 
oscillatory  error  in  the  early  portion  of  the  estimation  problem.  This  error  was 
unacceptable  and  a  new  initialization  procedure  was  investigated. 

The  basic  problem  with  the  initialization  of  the  SDREF  is  that  the  initial 
covariance  matrix  is  computed  using  the  solution  of  the  algebraic  Ricatti  equation.  This 
indicates  that  no  large  errors  are  expected.  Therefore,  there  is  no  method  of  supplying  the 
SDREF  with  an  initial  covariance  error  of  the  magnitude  of  the  expected  initialization 
errors.  Because  of  this  the  SDREF  computes  a  smaller  magnitude  covariance  matrix  and 
therefore  a  smaller  gain  to  be  used  in  the  state  update.  This  is  why  the  filter  oscillates 
during  the  initial  portion  of  the  flight.  If  the  SDREF  could  be  "jump-started"  with  a  high 
initial  covariance,  the  majority  of  the  initialization  error  could  be  taken  out  in  the  first 
time  step. 
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Figure  17.  EKF  position  errors  with  ±3c7standard  deviations 
for  1-percent  initialization  error. 
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Figure  18.  SDREF  position  errors  with  ±3erstandard  deviations 
for  1 -percent  initialization  error. 
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Figure  19.  EKF  position  errors  with  ±3<rstandard  deviations 
for  5-percent  initialization  error. 
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Figure  20.  SDREF  position  errors  with  ±3cstandard  deviations 
for  5-percent  initialization  error. 
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Table  3.  EKF  and  SDREF  parameters  five-percent  initialization  error 


Parameters 

(units) 

X 

Y 

Z 

Aspect  Angle 

(degrees) 

30 

30 

30 

SIGP 

(m) 

5.0E3 

168.0 

112.0 

SIGVP 

(m/s) 

5.0E2 

16.8 

11.2 

SIGXHAT 

rV 

(m2) 

25.0E6 

3.0E4 

3.0E4 

<TV2 

(m2/s2) 

25.0E4 

3.0E2 

3.0E2 

°t 

(m2/s4) 

1.0E1 

1.0E1 

1.0E1 

XHAT 

R 

(m) 

1.0E5 

10.0 

10.0 

V 

(m/s) 

-1.0E4 

-335.9 

223.9 

A 

(m/s2) 

55.2 

57.4 

-38.2 

SIGQ  (EKF) 

(m2/s4) 

196.0 

196.0 

196.0 

SIGQ  (SDREF) 

(m2/s4) 

1.0E6 

1.0E3 

1.0E3 

Meas  Noise 

(%,fim,jum) 

0.1 

50.0 

50.0 

If  the  gain  calculation  used  in  both  the  EKF  and  SDREF  is  examined, 

Kt  =  Pt(-)HkT[HtPt(-)HkT  +  Rk]-' 
it  can  be  seen  that  the  covariance  used  at  time  step  zero,  />,(-),  is  needed  in  the 
calculation.  If  it  is  assumed  that  the  value  of  Pk  that  the  SDREF  computes  is  Pt  (+)  (i.e., 
after  the  updated  measurement  has  occurred),  then  the  SDREF  is  initialized  with  Pt  (-)  to 
be  used  at  time  step  zero.  Using  this  methodology,  the  SDREF  was  initialized  with  the 
same  covariance  as  the  EKF  and  the  SDREF  process  noise  was  also  reduced  to  the  same 
level  as  that  of  the  EKF.  This  change  in  methodology  had  positive  impacts  on  the 
performance  of  the  SDREF  when  confronted  with  initialization  errors.  Figures  21 
through  26  show  the  new  performance  of  the  SDREF  using  the  same  initialization  errors 
as  in  the  baseline  runs.  It  can  be  seen  that  the  initial  oscillatory  motion  in  the  state 
estimate  errors  has  been  corrected  and  the  filter  compares  favorably  with  that  of  the  EKF. 
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Figure  21 .  SDREF  position  errors  with  ±3(7standard  deviations 
after  new  initialization  method,  1 -percent  error. 
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Figure  22.  SDREF  velocity  errors  with  ±3<Tstandard  deviations 
after  new  initialization  method,  1 -percent  error. 
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Figure  23.  SDREF  acceleration  errors  with  ±3crstandard  deviations 
after  new  initialization  method,  1 -percent  error. 
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Figure  24.  SDREF  position  errors  with  +3rjstandard  deviations 
after  new  initialization  method,  5-percent  error. 
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Figure  25.  SDREF  velocity  errors  with  ±3crstandard  deviations 
after  new  initialization  method,  5-percent  error. 
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Figure  26.  SDREF  acceleration  errors  with  ±3crstandard  deviations 
after  new  initialization  method,  5-percent  error. 
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This  new  initialization  methodology  not  only  improves  the  performance  in  the  presence 

of  initial  errors,  but  also  allows  a  convenient  way  of  initializing  the  SDREF. 

6.3.2    Measurement  Errors 

This  section  investigates  the  performance  of  the  EKF  and  SDREF  when  subjected 
to  various  degrees  of  measurement  noise.  The  relative  performance  of  the  two  filters 
using  the  baseline  measurement  noise  was  shown  in  Figures  3  through  5.  Using  the  same 
baseline  initial  conditions  given  in  Table  1 ,  two  other  levels  of  measurement  noise  were 
added  (Tables  4  and  5). 

Table  4.  EKF  and  SDREF  measurement  noise  parameters  level  -  1 


Parameters 

X                      Y                      Z 

Measurement  Noise      (%,  rad,  rad) 

1.0 

100.0E-6 

100.0E-6 

Filter  Noise  Variance 

(.01*  Rangef 

1.0E-8 

1.0E-8 

Table  5.  EKF  and  SDREF  measurement  noise  parameters  level  -  2 


Parameters 

X 

Y 

Z 

Measurement  Noise      (%,  rad,  rad) 

10.0 

500.0E-6 

500.0E-6 

Filter  Noise  Variance 

(0.1*  Rangef 

2.5E-7 

2.5E-7 
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The  second  level  of  noise  is  large  for  a  typical  engagement;  however,  it  exercises 

the  robustness  of  each  filter.  Both  filters  were  given  measurement  noise  covariance 

matrices  that  matched  the  noise  characteristics  given  to  the  sensor.  The  position  state 

estimate  plots  are  given  in  Figures  27  through  30.  There  is  not  a  significant  difference  in 

filter  performance  using  the  large  noises.  As  expected,  the  estimates  for  both  filters  are 

noisier  and  more  erratic.  The  acceleration  state  estimate  has  become  even  worse  under 

these  conditions.  This  estimate  was  initially  poor,  and  with  noisy  measurements  the 

filters  do  not  have  enough  information  to  estimate  the  target  acceleration  to  the  desired 

degree  of  accuracy.  It  can  also  be  noted  that  the  SDREF  estimation  error  under  the 

second  level  of  noise  seems  to  be  worse  than  the  EKF  estimate.  While  these  figures  only 

represent  one  run,  the  SDREF  seems  capable  of  adequate  performance  even  under 

stressing  noise  conditions. 

6.3.3  Target  Maneuvers 

This  section  investigates  the  open-loop  performance  of  the  EKF  and  SDREF 
against  two  stressing  target  maneuvers.  The  capabilities  of  the  filters  against  a  thrust  cut- 
off maneuver  have  been  previously  discussed.  In  this  section  two  other  nonlinear 
maneuvers  were  tested.  The  first  is  a  10-degrees  per  second  rotating  target  maneuver. 
This  maneuver  allows  the  target  acceleration  vector  to  rotate  approximately  1 10  degrees 
during  the  engagement.  This  is  a  moderately  high  target  maneuver,  but  it  is  possible  for 
an  ICBM  to  perform  this  type  of  action  in  an  active  countermeasure  scenario.  The  second 
stressing  engagement  consists  of  a  dogleg  maneuver  in  which  the  target  acceleration 
vector  rotates  90  degrees  out  of  plane  and  then  reverses  itself  to  rotate  back  inward  90 
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Figure  27.  EKF  position  errors  with  ±3<7standard  deviations 
1 -percent  error  in  X,  100  microradian  error  in  Y  and  Z. 
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Figure  28.  SDREF  position  errors  with  +3crstandard  deviations 
1 -percent  error  in  X,  100  microradian  error  in  Y  and  Z. 
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Figure  29.  EKF  position  errors  with  ±3cr  standard  deviations 
10-percent  error  in  X,  500  microradian  error  in  Y  and  Z. 
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Figure  30.  SDREF  position  errors  with  ±3crstandard  deviations 
10-percent  error  in  X,  500  microradian  error  in  Y  and  Z. 
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degrees.  The  initial  conditions  used  for  this  analysis  were  the  same  as  the  baseline 

scenario  except  for  the  process  noise  characteristics  given  to  the  filters  (see  Table  6). 
Table  6.  Maneuvering  target  parameters 


Parameters 

(units) 

X 

Y 

Z 

Aspect  Angle 

(degrees) 

30 

30 

30 

SIGP 

(m) 

0 

0 

0 

SIGVP 

(m/s) 

0 

0 

0 

SIGXHAT 

o? 

(m2) 

1.0E5 

1.0E5 

1.0E5 

<7„2 

(m2/s2) 

1.0E3 

1.0E3 

1.0E3 

<V 

(m2/s4) 

1.0E2 

1.0E2 

1.0E2 

XHAT 

R 

(m) 

1.0E5 

10.0 

10.0 

V 

(m/s) 

-1.0E4 

-335.9 

223.9 

A 

(m/s2) 

55.2 

57.4 

-38.2 

SIGQ  (EKF) 

(m2/s4) 

866.0 

866.0 

866.0 

SIGQ  (SDREF) 

(m2/s4) 

1.0E3 

1.0E3 

1E3.0 

Meas  Noise 

(%,jum,fjm) 

0.1 

50.0 

50.0 

The  process  noise  levels  were  tuned  to  allow  the  filters  to  open  up  and  "accept"  the 
unexpected  target  maneuvers.  While  this  allows  for  better  tracking,  it  does  lead  to  a 
noisier  state  estimate  in  both  filters.  Figures  31  through  34  show  the  relative  performance 
of  each  filter  against  these  two  scenarios. 

As  can  be  seen  from  these  figures,  there  are  no  appreciable  differences  in  the  state 
estimates  between  the  two  filters.  They  each  track  this  maneuverable  target,  however  the 
state  estimate  error  is  noisier  and  larger,  as  expected.  It  should  be  noted  that  once  the 
initial  transient  terms  in  the  EKF  covariance  have  settled,  they  are  similar  in  shape  and 
value  to  the  "steady-state"  covariance  terms  of  the  SDREF. 
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Figure  31.  EKF  position  errors  with  ±3(jstandard  deviations 
for  10  deg/sec  rotating  target  maneuver. 
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Figure  32.  SDREF  position  errors  with  ±3<rstandard  deviations 
for  10  deg/sec  rotating  target  maneuver. 
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Figure  33.  EKF  position  errors  with  ±3<r  standard  deviations 
for  dogleg  target  maneuver. 
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Figure  34.  SDREF  position  errors  with  ±3c  standard  deviations 
for  dogleg  target  maneuver. 
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6.4  SDREF  Closed  Loop  Performance 

6.4.1  Other  Engagement  Aspect  Angles 

Up  to  this  point  the  EKF  and  SDREF  performance  in  an  open-loop  environment 
have  been  examined.  The  following  analysis  examines  the  closed-loop  performance  of 
each  filter  against  various  engagement  scenarios.  This  includes  a  variety  of  engagement 
aspect  angles,  target  maneuvers,  initial  condition  errors,  and  measurement  noise  levels. 
In  this  section,  however,  performance  in  terms  of  probability  of  hit  and  percentage  of  fuel 
used  to  achieve  the  hit  are  measured.  Measuring  the  percentage  of  fuel  used,  allows  a 
comparison  of  how  noisy  the  state  estimates  are,  as  increased  error  in  the  beginning  of  the 
engagement  results  in  fuel  being  wasted  performing  unnecessary  maneuvers.  This 
analysis  was  performed  using  31  Monte  Carlo  run  statistics.  The  first  analysis  focused  on 
three  engagement  scenarios.  Up  to  this  point  a  30,30,30-degree  closing  aspect  angle  was 
used.  A  head-on,  90-degree  beam  shot,  and  a  tail-chase  engagement  are  now  added. 
Figure  35  shows  the  three  engagements  and  the  corresponding  simulation  parameters  are 
given  in  Tables  7  through  9. 
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Table  7.  Head-on  simulation  parameters 


Parameters 

(units) 

X 

Y 

Z      1 

Aspect  Angle 

(degrees) 

30 

30 

30 

SIGP 

(m) 

0 

0 

0 

SIGVP 

(m/s) 

0 

0 

0 

SIGXHAT 

<J. 

(m2) 

1.0E5 

1.0E5 

1.0E5 

a,1 

(m2/s2) 

1.0E3 

1.0E3 

1.0E3 

°? 

(m2/s4) 

1.0E2 

1.0E2 

1.0E2 

XHAT 

R 

(m) 

1.0E5 

10.0 

10.0 

V 

(m/s) 

-10000.0 

-335.9 

223.9 

A 

(m/s2) 

55.2 

57.4 

-38.2 

SIGQ  (EKF) 

(m2/s4) 

866.0 

866.0 

866.0 

SIGQ  (SDREF) 

(m2/s4) 

1.0E3 

1.0E3 

1.0E3 

Meas  Noise 

(%,,um,/mi) 

0.1 

50.0 

50.0 

Table  8.  90-degree  beam  shot  simulation  parameters 


Parameters 

(units) 

X 

Y 

Z 

Aspect  Angle 

(degrees) 

0 

0 

-90 

SIGP 

(m) 

0 

0 

0 

SIGVP 

(m/s) 

0 

0 

0 

SIGXHAT 

a,2 

(m2) 

1.0E5 

1.0E5 

1.0E5 

<y* 

(m2/s2) 

1.0E3 

1.0E3 

1.0E3 

"t 

(m2/s4) 

1.0E2 

1.0E2 

1.0E2 

XHAT 

R 

(m) 

1.0E5 

10.0 

10.0 

V 

(m/s) 

-1.0E4 

498.2 

0.0 

A 

(m/s2) 

0.0 

-88.3 

0.0 

SIGQ  (EKF) 

(m2/s4) 

196.0 

196.0 

196.0 

SIGQ  (SDREF) 

(m2/s4) 

196.0 

196.0 

196.0 

Meas  Noise 

(%,jum,jum) 

0.1 

50.0 

50.0 
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Table  9.  Tail-chase  simulation  parameters 


Parameters 

(units) 

X 

Y 

Z 

Aspect  Angle 

(degrees) 

0 

150 

0 

SIGP 

(m) 

0 

0 

0 

SIGVP 

(m/s) 

0 

0 

0 

SIGXHAT 

<rr2 

(m2) 

1.0E5 

1 .01:5 

1.0E5 

<rv2 

(m2/s2) 

1.0E3 

1.0E3 

1.0E3 

°t 

(m2/s4) 

1.0E2 

1.0E2 

1.0E2 

XHAT 

R 

(m) 

1.0E5 

10.0 

10.0 

V 

(m/s) 

-1.0E4 

0.0 

-237.9 

A 

(m/s2) 

-76.5 

0.0 

-44.1 

SIGQ  (EKF) 

(m2/s4) 

196.0 

196.0 

196.0 

SIGQ  (SDREF) 

(m2/s4) 

196.0 

196.0 

196.0 

Meas  Noise 

(%,//m,^m) 

0.1 

50.0 

50.0 

The  90-degree  beam  shot  is  the  most  stressing  of  these  engagements  as  the  entire 
target  acceleration  profile  is  out  of  plane.  The  head-on  case  is  one  of  the  easier 
engagements  when  using  a  range  measurement,  and  the  tail  chase  lies  in-between  these 
two  in  terms  of  estimation  difficulty.  Table  10  displays  the  results  from  the  Monte  Carlo 
analysis  against  these  engagements.  Probability  of  hit  (PHIT)  is  the  probability  that  the 
interceptor  hit  the  target.  Circular  error  probable  (CEP)  is  the  probability  that  50-percent 
of  the  misses  lie  within  this  bound,  95-percent  miss  includes  ninety-five  percent  of  the 
misses,  and  worst  miss  displays  the  worst  of  the  Monte  Carlo  runs.  The  mass  usage 
numbers  reflect  the  same  quantities  except  in  terms  of  percent  of  divert  fuel  used.  The 
vehicle  has  a  mass  of  4  kilograms,  1.5  of  which  is  devoted  to  divert  fuel.  When  this  limit 
is  exceeded,  the  interceptor  will  continue  to  fly  towards  the  target  with  no  further 
guidance  corrections,  typically  resulting  in  large  miss  distances.  Since  these  engagements 
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had  good  initial-state  estimates  and  the  target  is  not  maneuvering,  all  of  the  runs  were 

hits;  however  as  shown  in  the  following  sections  that  this  is  not  always  the  case. 
Table  10.  Miss  distance  performance  for  varying  scenarios 


Parameter   (units) 

0°                                90°                              150° 
EKF         SDREF        EKF         SDREF      EKF         SDREF 

PHIT 

1.0 

1.0 

1.0 

1.0 

1.0 

1.0 

CEP                 (m) 

0.185 

0.262 

0.432 

0.625 

0.315 

0.265 

Mean  Miss       (m) 

0.213 

0.272 

0.546 

0.667 

0.347 

0.354 

95%  Miss         (m) 

0.364 

0.426 

1.212 

1.209 

0.654 

0.671 

Worst  Miss      (m) 

0.421 

0.599 

1.828 

1.679 

0.803 

0.895 

Mass  CEP        (%) 

16.70 

27.61 

67.38 

72.67 

43.42 

48.06 

Mass  Mean      (%) 

16.90 

28.27 

66.98 

72.76 

43.11 

48.40 

Mass  95%        (%) 

21.03 

32.78 

69.25 

75.22 

46.10 

51.62 

Mass  Worst     (%) 

22.70 

33.16 

71.20 

75.51 

48.57 

52.59 

From  Table  10,  no  significant  difference  in  the  closed-loop  performance  of  these  two 
filters  under  the  above  scenarios  is  shown.  All  performed  adequately  and  ensured  a  hit 
against  the  target  in  all  cases.  Fuel  usage  was  also  not  a  decisive  factor  as  both  filters 
differed  by  only  a  few  percentage  points. 
6.4.2  Stressing  Target  Maneuvers 

To  further  test  filter  performance,  two  other  target  maneuvers  were  considered. 
These  maneuvers  are  on  the  outer  edge  of  filter  capability  of  properly  tracking  the  "true" 
state.  They  were  included  to  investigate  whether  the  parameterization  of  the  SDREF 
allows  an  advantage  over  the  linearization  of  the  EKF  in  highly  nonlinear  environments. 
The  first  maneuver  is  a  rotating  target,  however  in  this  engagement  the  engagement 
aspect  angle  was  a  90-degree  beam  shot  and  the  rotation  rate  was  increased  to  20  degrees 
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per  second.  Therefore,  the  target  will  have  completed  an  approximately  200-degree  turn 

during  the  length  of  the  engagement. 

The  second  maneuver  consisted  of  a  dogleg  evasive  maneuver.  This  maneuver  is 
similar  to  the  previously  described  dogleg  maneuver  except  that  it  was  performed  in  the 
90-degree-beam-shot  scenario,  causing  it  to  become  more  difficult  for  the  filter  to 
estimate. 

Table  1 1  displays  the  results  of  Monte  Carlo  runs.  The  process  noise  levels  for 
each  filter  were  tuned  to  give  the  best  performance  against  these  scenarios. 

Table  1 1 .  Miss  distance  performance  for  stressing  target  maneuvers 


Parameters  (units) 

207Sec 
EKF         SDREF 

Dogleg 
EKF             SDREF 

pun 

0.935 

0.806 

0.871 

0.742 

CEP                (m) 

1.61 

2.20 

1.14 

2.85 

Mean  Miss      (m) 

1.83 

200.20 

1.50 

3.05 

95%  Miss        (m) 

4.30 

7.44 

3.94 

6.95 

Worst  Miss      (m) 

6.00 

6121.89 

5.59 

7.36 

Mass  CEP      (%) 

100 

100 

100 

100 

Mass  Mean     (%) 

99.94 

99.99 

99.87 

99.98 

Mass  95%       (%) 

100 

100 

100 

100 

Mass  Worst     (%) 

100 

100 

100 

100 

As  shown  in  Table  1 1 ,  the  SDREF  performed  slightly  worse  against  the  targets  than  the 
EKF.  The  one  large  miss  distance  for  the  SDREF  was  caused  by  poor  tracking,  which 
caused  the  interceptor  to  run  out  of  fuel  early  and  coast  to  a  large  miss  distance. 

The  purpose  of  this  section  was  to  stress  the  filters  to  their  extreme  operational 
limits  and  investigate  the  behavior.  These  types  of  target  maneuvers  would  be  considered 
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unlikely.  However,  they  do  provide  a  convenient  way  to  determine  the  robustness  of  each 

filter  design.  Even  at  these  extremes,  the  SDREF  was  almost  able  to  meet  the  EKF 

performance  measures. 

6.4.3  Measurement  Errors 

This  paragraph  discusses  filter  sensitivity  to  various  levels  of  measurement  error. 
For  this  analysis  the  range  measurement  error  was  held  constant  at  1  percent  due  to  the 
size  of  the  range  error  when  dealing  with  large  engagement  distances.  Azimuth  and 
elevation  noises  were  included  for  100,  500,  and  1000  microradian  errors.  The  results 
displayed  in  Table  12  were  done  using  a  31 -run  Monte  Carlo  experiment. 

Table  12.  Miss  distance  performance  for  varying  measurement  noise 


Parameters  (units) 

100/ffad 
EKF        SDREF 

500,urad 
EKF         SDREF 

1000 /ttad 
EKF        SDREF 

PHIT 

1.0 

1.0 

0.968 

1.0 

0.935 

0.806 

CEP                 (m) 

0.455 

0.460 

1.39 

1.41 

3.09 

2.22 

Mean  Miss       (m) 

0.468 

0.568 

1.47 

1.45 

3.17 

3.14 

95%  Miss         (m) 

0.775 

1.218 

2.47 

2.60 

5.66 

6.29 

Worst  Miss      (m) 

0.935 

1.569 

2.55 

3.08 

6.03 

8.18 

Mass  CEP        (%) 

77.13 

76.86 

84.19 

81.83 

89.79 

86.99 

Mass  Mean      (%) 

77.57 

76.96 

84.66 

82.36 

90.23 

87.05 

Mass  95%        (%) 

80.67 

78.95 

88.27 

85.12 

94.46 

89.40 

Mass  Worst     (%) 

81.23 

79.55 

90.92 

86.31 

96.66 

92.67 

Once  again,  the  results  between  the  two  filters  are  similar.  Even  with  high-measurement 
noise  levels,  both  filters  performed  adequately,  missing  only  one  or  two  times  in  the 
Monte  Carlo  run.  The  fuel  usage  numbers  are  also  comparable.  These  results  verify  the 
results  of  the  pdf  comparison  between  the  filters,  as  the  probability  distributions  are 
similar  between  the  EKF  and  SDREF.  Therefore,  it  can  be  assumed  that  the  filters  are 
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Monte  Carlo  run.  The  fuel  usage  numbers  are  also  comparable.  These  results  verify  the 

results  of  the  pdf  comparison  between  the  filters,  as  the  probability  distributions  are 
similar  between  the  EKF  and  SDREF.  Therefore,  it  can  be  assumed  that  the  filters  are 
using  approximately  the  same  set  of  distribution  functions,  even  though  they  are  achieved 
in  different  ways. 
6.4.4  Initialization  Errors 

Using  the  new  initialization  method  described  in  section  6.3. 1 ,  a  closed-loop 
analysis  of  the  EKF  and  SDREF  was  performed.  The  initial  conditions  used  for  the  open- 
loop  initialization  study  were  used  for  this  investigation.  The  filters  were  run  closed-loop 
in  a  31 -run  Monte  Carlo  simulation.  Table  13  displays  the  results  of  these  runs.  Using 
this  new  initialization  method  not  only  shows  better  performance  in  the  single-run,  open- 
loop  case,  but  performs  equally  as  well  in  the  closed-loop  analysis.  Probability  of  hit, 
miss  distance,  and  fuel-usage  numbers  show  that  the  SDREF  performance  is  similar  to 
that  of  the  EKF. 


Table  13  -  Miss  distance  performance  for  varying  initialization  error 


Parameters  (units) 

1%  Error 
EKF            SDREF 

5%  Error 
EKF         SDREF 

PHIT 

1.0 

1.0 

1.0 

1.0 

CEP 

(m) 

0.436 

0.377 

0.336 

0.389 

Mean  Miss 

(m) 

0.464 

0.380 

0.423 

0.432 

95%  Miss 

(m) 

0.849 

0.750 

0.858 

0.770 

Worst  Miss 

(m) 

1.01 

0.793 

1.13 

1.12 

Mass  CEP 

(%) 

75.13 

79.18 

72.53 

92.03 

Mass  Mean 

(%) 

75.00 

79.45 

72.53 

92.03 

Mass  95% 

(%) 

77.34 

81.92 

74.56 

93.20 

Mass  Worst 

(%) 

78.12 

84.78 

75.55 

93.42 

87 
6.5  Summary  of  Exoatmospheric  Guidance  Problem 

The  exoatmospheric  guidance  problem  analysis  demonstrated,  with  some  minor 
caveats,  the  SDREF  was  capable  of  satisfying  the  interceptor  guidance  problem.  The 
assumptions  of  detectability  in  solving  the  SDRE  required  the  addition  of  a  range 
measurement.  An  ad  hoc  initialization  algorithm  needed  to  be  developed  to  properly 
initialize  the  SDREF.  With  these  two  restrictions  in  place,  however,  the  SDREF 
performed  similarly  to  the  EKF  for  the  exoatmospheric  guidance  problem  for  several 
engagements,  noise  sources,  and  initial-error  combinations. 


CHAPTER  7 
PENDULUM  PROBLEM  FILTER  DEVELOPMENT 

To  evaluate  the  effect  of  nonlinear  dynamics  on  the  SDREF  performance,  a  simple 
two-dimensional  pendulum  problem  was  constructed.  The  scenario  consists  of  a  simple 
rigid  pendulum  with  friction  about  the  connection  point,  see  (Figure  36).  The 
measurements  are  assumed  to  come  from  an  accelerometer  attached  to  the  bob,  measuring 

e. 


Figure  36.  Pendulum  set-up. 
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The  pendulum  is  given  some  initial  position  and  velocity  and  the  friction  term 

damps  the  oscillations.  This  system,  while  simple,  demonstrates  nonlinear  dynamics  and 

nonlinear  measurements  together  in  the  same  problem. 

The  system  dynamics  for  the  pendulum  problem  are  given  by  the  2nd  order 

nonlinear  differential  equation 

G  =  --sin0-B0  (7.1) 

where  B0  is  the  friction  term.  B  was  set  to  be  0.5. 

Both  the  SDREF  and  EKF  were  derived  using  the  stochastic  nonlinear  system 


x  =  f(x)  +  Tw 
z  =  h(x)  +  v 

where  w  and  v  are  Gaussian,  zero-mean  white-process  and  measurement  noise, 

respectively,  given  by 


E[w(t)wT(t  +  T)  =  W{t)S(T)] 
E[v{t)v(t  +  T)  =  R(t)S(T)} 


xx=Q 
x2=Q 

then  the  state  equations  for  the  pendulum  system  are 
x2- sin(  xt )  -  Bx2 


(7.2) 


(7.3) 


(7.4) 


(7.5) 


The  measurement  equation  is 

z  =  -—sin{xl)-Bx1  (7.6) 


Therefore, 


/(*)  = 

h(x)  = 


90 

0  x2 

-smx.     -Bx, 


—  sin*.     -Bx, 
L        ' 


(7.7) 


7.1  SDREF  Derivation 

Using  the  state-dependent  coefficient  parameterization  technique  described  in  the 
exoatmospheric  guidance  problem,  the  state  equations  can  be  brought  to  the  state- 
dependent  linear-like  form  with 


K(x)  =  P(x)HT(x)R-'F(x)-- 


0 
g  sin(jT|) 

L     x. 


1 
-B 


and 


H(x)  = 


g  sin(.v,) 
L       v, 


-B 


(7.8) 


(7.9) 


and  the  resulting  filter  equations  become 

x  =  F(x)x  +  K(x)[z(x)-H(x)x\  (7.10) 

where  K(x)  =  P(x)HT(x)R~'  (7.11) 

and  P(i)  is  the  solution  of  the  state-dependent  Ricatti  equation 

F(x)P  +  PFT(x)-PHT(x)R]H(x)P  +  TTWr  =  0  (7.12) 

Although  this  system  only  has  two  states  and  seems  simple,  a  closed  form  solution 
of  the  Ricatti  equation  proves  difficult.  After  hours  of  computation  with  the  symbolic 
solver  Macsyma,  a  closed-loop  solution  was  determined.    However,  the  three  equations 
for  Pu,Pl2,  and  P22  involved  over  300  lines  of  computations.  Although  these 
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calculations  provided  the  correct  result,  it  was  determined  that  the  previous  method  used 

for  computing  the  solution  to  the  guidance  problem's  Ricatti  equation  would  be  used.  It 

consisted  of  a  continuous  SDRE  solver  obtained  from  the  Lin-Pak  algorithm  set  available 

on  the  world-wide-web.  It  was  easier  to  incorporate  into  the  pendulum  simulation,  and 

executed  faster. 


7.2  Extended  Kalman  Filter  Derivation 

The  EKF  uses  the  same  stochastic  system  as  the  SDREF,  equations  (7.2)  through 
(7.6).  However,  the  dynamic  and  measurement  matrices  are  linearized  instead  of  being 
parameterized  as  in  the  SDREF.  The  linearized  equations  are  given  by: 


.  df(x) 
dx 


and 


H  = 


dh(x) 


0 
p 

COSJC, 

L         ' 


cosx, 

L         ' 


-B 


So  the  filter  equations  are 

x  =  f(x)  +  PHTR-'[z-h(x)] 
where  P  =  FP  +  PFT  -  PHT R'' HP  +  TTWT 


(7.13) 

(7.14) 

(7.15) 
(7.16) 


CHAPTER  8 
PENDULUM  PROBLEM  SIMULATION  RESULTS 

The  pendulum  problem  was  analyzed  in  the  visual  simulation  tool,  VisSim.  The 

model  used  consisted  of  the  following  parameters. 

L=1.0(ft) 

g  =  32.2  (ft  /sec2) 

(8.1) 
".05     0 " 


rwr  = 


(rad/sec  ) 


0     .05 

The  simulation  was  run  using  fourth-order  Runge-Kutta  integration  at  a  rate  of  2,000 
hertz  (Hz). 

The  first  analysis  was  performed  with  both  filters  receiving  the  true  initial  conditions 

as  their  initial  estimates.  The  initial  conditions  used  were 

x,  =  1.0  rad  *,  =  1.0  rad 

x2  =  5.0  rad  /  sec        x2  =  5.0  rad  /  sec 

Figures  37  and  38  show  the  results  of  this  run.  As  can  be  seen  in  the  figures,  both 

filters  performed  well  for  this  scenario.  Figures  39  and  40  show  that  the  errors  for  both 

filters  are  almost  identical.  The  goal  of  running  this  pendulum  problem,  however,  was  to 

determine  if  the  pdf  functions  for  the  two  filters  were  similar  when  nonlinear  dynamics 

were  incorporated  with  the  nonlinear  measurements.  The  PDF  curve  for  each  filter, 

Figures  41  through  46,  was  determined  at  three  time  locations,  0.5,  2.0,  and  3.0  seconds 

into  the  simulation.  These  distributions  were  computed  in  the  same  manner  as  explained 
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Figure  37.  EKF  and  SDREF  estimates  vs.  true  theta 
with  no  initialization  error. 


Figure  38.  EKF  and  SDREF  estimates  vs.  true  theta-dot 
with  no  initialization  error. 
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Figure  39.  EKF  and  SDREF  estimation  error  for  theta 
with  no  initialization  error. 
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Figure  40.  EKF  and  SDREF  estimation  error  for  theta-dot 
with  no  initialization  error. 
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Figure  41.  Monte  Carlo  probability  density  function  for 
theta,  with  no  initialization  error,  time  =  0.5  sec. 
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Figure  42.  Monte  Carlo  probability  density  function  for 
theta-dot,  with  no  initialization  error,  time  =  0.5  sec. 
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Figure  43.  Monte  Carlo  probability  density  function  for 
theta,  with  no  initialization  error,  time  =  2.0  sec. 
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Figure  44.  Monte  Carlo  probability  density  function  for 
theta-dot,  with  no  initialization  error,  time  =  2.0  sec. 
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Figure  45.    Monte  Carlo  probability  density  function  for 
theta,  with  no  initialization  error,  time  =  3.0  sec. 
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Figure  46.  Monte  Carlo  probability  density  function  for 
theta-dot,  with  no  initialization  error,  time  =  3.0  sec. 
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earlier  in  the  guidance  problem,  Section  6.2.  Figures  41  through  46  show,  the  curves 

again  are  similar.  The  second  analysis  examined  any  differences  resulting  when  an  initial 

error  was  given  to  each  filter.  The  initial  conditions  used  were 

x,  =  0.0  rad  x,  =  0.9  rad 

x2  =  5.0  rad  /  sec        ic2  =  5.0  rad  /  sec 

As  was  evident  in  the  guidance  problem,  the  SDREF  pdf  mirrors  the  EKF  pdf  once 
the  filter  has  settled.  Figures  47  and  48  show  the  initial  settling  differences  in  the  SDREF 
and  EKF.  The  SDREF  is  slower  to  settle,  just  as  in  the  guidance  problem.  However, 
once  settled,  the  performance  matches  that  of  the  EKF.  Once  again  Figures  49  through 
54  demonstrate  the  similarity  in  the  pdf  shape  between  the  two  filters. 

This  study  helps  to  confirm  the  results  of  the  guidance  analysis.  The  SDREF,  while 
derived  in  a  different  manner,  leads  back  to  the  same  internal  representation  of  the  pdf  as 
that  of  the  EKF  once  the  initial  uncertainty  has  settled. 
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Figure  47.  EKF  and  SDREF  theta  estimation  error 
with  initialization  error. 
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Figure  48.  EKF  and  SDREF  theta-dot  estimation  error 
with  initialization  error. 
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Figure  49.  Monte  Carlo  probability  density  function  for 
theta,  with  initialization  error,  time  =  0.5  sec. 
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Figure  50  -  Monte  Carlo  probability  density  function  for 
theta-dot,  with  initialization  error,  time  =  0.5  sec. 
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Figure  5 1 .    Monte  Carlo  probability  density  function  for 
theta,  with  initialization  error,  time  =  2.0  sec. 
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Figure  52.  Monte  Carlo  probability  density  function  for 
theta-dot,  with  initialization  error,  time  =  2.0  sec. 
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Figure  53.  Monte  Carlo  probability  density  function  for 
theta,  initialization  error,  time  =  3.0  sec. 
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Figure  54.  Monte  Carlo  probability  density  function  for 
theta-dot,  initialization  error,  time  =  3.0  sec. 


CHAPTER  9 
MSDREF  DEVELOPMENT 

Although  the  SDREF  investigated  to  this  point  has  demonstrated  adequate 
performance,  no  proof  for  stability  could  be  determined.  A  modification  to  the  filter 
algorithm  was  found  that  allowed  a  stability  proof  to  be  formulated.  It  involved  removing 
the  infinite-time  horizon  assumption  in  the  computation  of  the  covariance  matrix. 

9.1    MSDREF  Derivation 

Consider  the  continuous  system 

x  =  f(x)  +  Gw 
=  F(x)x  +  Gw  (9,1) 

Z  =  h(x)  +  v 
=  H(x)x  +  v  (9'2) 

where  w  is  Gaussian  zero-mean  white  process  noise  with  E[w(t)wT(t  +  T)]  =  Q(t)S(T) 
and  v  is  Gaussian  zero-mean  white  measurement  noise  with£[v(/)vr(r  +  r)]  =  R(t)S(r) . 
Also  assume  that  the  MSDREF  is  one  where  the  state  update  and  covariance  update  are 
computed  simultaneously,  as  the  covariance  equation  is  state  dependent 
Then  the  equations 

x  =  F(x)x  +  K(x)(z-H{x)x)  (9.3) 

K=PHT(x)R'  (9.4) 

P  =  F(x)P  +  PFT(x)  +  GQGT-KRKT  (9.5) 
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define  the  continuous  MSDREF.  The  system  dynamics  and  filter  equations  can  be 

rewritten  in  discrete  form  as 

*t+i=*»Jt*+w»  (9-6) 

z*+,  =*(%►,) +vt+1  (9.7) 

*ltI/i=**,  (9.8) 

Xk+l  ~  Xk+\lk  +  *n+l|.Z*+l  ~  ™k+\(Xk+\/k)Xk+Vk  J  ("■") 

PW(-)«*»W+&  (9-11) 

Pt+,(+)  =  [/-tft//JPt(-)  (9.12) 

with  E[vMv4+Ir]  =  J?t+!  (9.13) 

E[w,+,wt+,r]  =  ei+,  (9.14) 

Equations  (9.8-9.12)  define  the  discrete  MSDREF.  It  is  also  assumed  that  vt+]  and  wt+l 
are  random,  zero-mean  inputs  and  therefore,  stability  of  the  MSDREF  is  independent  of 
these  error  sources. 

9.2  Stability  Proof 

The  following  proof  shows,  under  the  conditions  of  nonlinear  and  "linear" 
observability,  that  the  MSDREF  is  locally  asymptotically  stable.  It  makes  use  of  a  similar 
proof  for  the  extended  Kalman  observer  (EKO),21  however  instead  of  using  the  error  due 
to  linearization  of  the  EKO,  the  proof  is  reformulated  to  account  for  H.O.T  terms  due  to  a 
Taylor  series  expansion  of  the  parameterized  equation  in  the  MSDREF. 
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Theorem: 

Consider  the  nonlinear  system  given  in  (9.1-9.2)  and  equations  (9.6-9.14),  if  we  assume 

a.  Ft,Hk  are  uniformly  bounded  matrices  and  Fk~  exists  (9.15) 

b.  the  nonlinear  system  isW-locally  uniformly  rank  observable;  i.e.,  there  exists 
an  integer  N  >  1  such  that 


rank- 


?h 


( 

ht(x)                 \ 

K+,  (/»(*)) 

A« 

-,(/t«-2  )•..(/*(*», 

(9.16) 


for  all  xt  e  K,  where  A'  is  a  compact  set  of  i?"and  hk+i  (/,  (*))  is  defined  by  the 

Lie  derivative. 

c.    the  parameterized  system  is  AMocally  uniformly  rank  observable  such  that 


rank 


Ht(xt) 
HMFt(xk) 


(9.17) 


l#i-+„-i  F*«-i— *i(*«), 

for  all  xt  e  K ,  where  K  is  a  compact  set  of  if" . 


d. 


1-Vl-At+I<a1(t,<l  +  A/1-At+ 
for    i  =  \,...,p 


(9.18 


where  Attl=  X^JR^X^iR^'H^P^H.jR^') 

and  Amj(.)  represents  the  maximum  eigenvalue  of  (.) 
e.  -VfT^^  <VT7     for     j  =  \,...,n 

where 

r  _^.(f>nf/+e>) 


(9.19) 
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Then  the  MSDREF  is  locally  asymptotically  stable. 

Proof: 

The  state  estimation  error  both  before  and  after  the  update  is  defined  as 

xt+1  =  xt+l  -  xkH        (after  update)  (9.20) 

st*iitmxiM-XiMn  (before  update)  (9.21) 

For  ease  of  notation  the  following  terms  are  defined  as 

ff.A)  =  fiW  (9.22) 

PM(xM)  =  Pn  (9.23) 

e*t,  =2M-HM(.xM)xM  (9.24) 

And  finally  a  candidate  Lyapunov  function  Vt+1  is  defined 

't+i=xt+i   "t+i   *t+r  (9.25) 

Remark  1 

For  equation  (9.25)  to  be  valid,  Pi  must  be  bounded  from  above  and  below.  A  test  for  the 
observability  of  the  nonlinear  system22  is 

hk{x) 

IA-M-1  fk+»-2---fkW)x 

Ifn  is  of  full  rank  for  all  xeR",  then  the  system  is  observable  for  all  x.  However, 
satisfying  (9.26)  does  not  guarantee  that  the  parameterized  system  used  in  the  MSDREF 
will  be  observable.      Therefore,  the  observability  of  the  parameterized  system  pair 
[F(x),  H(x)j  must  also  be  tested.  Then  not  only  has  the  nonlinear  system  been  shown  to 
be  observable;  so  that  a  filter,  for  which  all  states  are  observable,  can  exist  for  the 


rank  ■ 


dx 


(9.26) 
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system,  but  it  will  also  be  possible  to  use  the  parameterized  MSDREF  equations.  If  both 

rank  conditions  (9.16)  and  (9.17)  are  met,  the  condition  for  observability 

k 
PJ<  Yd®T(i*)H*(RRTyxHl<S>(i,k)<P1l  (9.27) 

i=k-M 

is  also  satisfied,  where  /?,  /?,  are  positive  real  numbers,  for  some  finite  M  >  0,  and  for 
all  k>  M,  and  Pk  is  bounded  from  above  and  below.24 

The  condition  for  which  V,+1  is  a  decreasing  sequence  must  now  be  determined, 
and  will  show  the  local  asymptotic  stability  of  the  MSDREF.  The  following  proof  uses21 
and  the  form  of  the  MSDREF  to  establish  a  theoretical  basis  for  stability. 

While  linearization  of  the  filter  equations  has  proven  unsuccessful  in  showing 
stability  of  the  filter  in  the  past,  this  approach  constrains  the  state  transition  and  the 
measurement  noise  matrices  so  as  to  characterize  the  error  due  to  parameterization.  The 
measurement  residual  error,  equation  (9.24)  can  be  written  as 

**«  =  #*+i(**«)**«-JW*»+!)*Wi  (9.28) 

For  each  output  error  prediction  component  of  ettl ,  namely  <?,  1+l ,  and  for  each  state  error 
prediction  component  of  xktllk ,  namely  xjMm\  i=  \,...,p  and  j  =  1,...,«  there  always 
exists  residues  riM  and  ojk  due  to  the  first  order  linearization  of  Hi  ,+1  ( jci+] )  and  F  ,  (jct ) 
in  order  to  obtain  an  exact  equality,  for  all  values  of  xk+]lk ,  xk  xM ,  xk,  such  that 

eiM\  ~   "i.k+]Xk+]/k  ~*~ri.k+\ 
Xj.kt\lk  =  Fft  +0j.k 

HiM  and  Fjk  are  the  ith  and  jlh  rows  of  Hkfl  and  Fk  respectively.  Expanding 
HiM\  (•*!+! ) m  a  Taylor  series  about  xM  and  using  the  Mean  Value  Theorem,  we  have 
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«W+i  =  Hi^i(*w)*t+i  -c0'M^a+iU*+i)) 

dcol'{HiM1^,M)}^  (9-29) 

+ 57 (**+, -**+,)  K+i         ^  =  1»— .» 

where  the  vector  £',.i+l  is  that  point  on  the  line  segment  joining  the  xk+1  and  *4+,  that 

yields  equality  in  the  j'h  equation  of  (9.29).  Let  the  residual  error  be  defined  as 

dcol>{HlMi(S',M)}^ 
riM,  = - (*t+l  -xM)}xM  (9.30) 

then  the  exact  measurement  error  can  be  defined  as 
or  rewritten  as 

where  ai  t+l  is  an  unknown,  time-varying  scalar  related  to  rli+l  by 

■Sw-a-Wkw  (933) 

Through  the  same  process,  the  equation  for  the  system  dynamics  is  obtained 

ZjMm  =  FjM*  +  °i.k  (9-34) 

where  7  refers  to  the  /*  component  and  oj  t  is  the  residual.  Written  in  another 
equivalent  form 

XlMm=bj.kFj,tXk  (9-35> 

where  b;  k  is  an  unknown  time  varying  scalar  related  to  ojk  by 

oik={b]k-\)F]kxk  (9.36) 

Recomposing  equations  (9.32)  and  (9.35)  into  vector  form  yields 

ak+\ek+l  ~  **k+\*t+Vt  ,q  -^ 

Xk+\/k  ~^k^kXk 
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where  at+,  6  Rppand  bk  e  R°"  is  an  unknown  time-varying  diagonal  matrix  defined  as 


bk=diag(blk,...,bnk) 


(9.38) 


If  both  sides  of  equation  (9.9)  are  subtracted  from  xM 

*w  =  %+i/i  ~  P^HMTRltl-'eM  (9.39) 

Substituting  this  into  equation  (9.27),  and  using  the  fact  that  PM  =  PkJ 


'k-tl        Xk+Vk     "i+I     Xk+Uk       Xk+Uk    "k+i    "*+I     ek  +  l 

_e*+i  Rk+i    Hk-nxk+uk+ek+\  rm   Hk+\Pk+\Hk+\  rm   ek+\ 


(9.40) 


Before  proceeding,  the  inverse  relationship  is  defined  as 

1«  l  =  Pk,ukX+HMTRM{HM  (9.41) 

through  a  matrix  inversion  relationship  for  PM{+)  as  given  in  equation  (9.17). 
Substituting  equation  (9.41)  into  (9.40), 


"»+i_%rtrt   lpt+\n    +#i+i  ^*+i   Hkn  }xMlk  -  xktl/k  HM  RM   ektX- 


and  using 


the  following  is  obtained 

"t+i  =Vk-u/k  +xk+\ik  "*+i  "t+i   #i+i^»+i/t  ~xk+\ik  Hk+\  "i+i   ek*i 
-ek,^k^'Hk+lxk,)lk+ekjRk+l''HktlPk^HMTRttl-'ektl 

Using  equation  (9.35),  equation  (9.44)  becomes 

vm  =VMik  +ekJ (aMRk+,~'aM-ak+iRMl  -  ^+r'at+i 


(9.42) 


(9.43) 


(9.44) 


(9.45) 
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Another  way  to  write  VktUk  is 


l-^\-AM<a,M]<\  +  ^A~k 
for    i  =  \,...,p 


(9.46) 


Using  the  definition  of  a  decreasing  sequence  as 

VM  -Vk=  V4+,  - VMlk  +VMll  -Vk  < 0  (9.47) 

then 

VM  ~Vk  =  ekuT(ak+1Rk+;laM  -aMRM'-RM'aM 

+  Rk^U^PMHjRM])eM  (9.48) 

+  xkTlFkTbk(FkPtFtT+QkV'bkFk-p-'t]xk<0 

A  sufficient  condition  to  ensure  asymptotic  stability  is  that 

«A\i  -^A^'-R^'a^+R^'H^P^H^R^'  <0  (9.49) 

^TFkTbk(FkPkFkT  +  Qky'bkFkxk-Pt-'  <0  (9.50) 

Conditions,  which  allow  this  to  be  true,  assume  that 


(9.51) 


with 

4m  =^„(tft+1U„»(Kt+,~X+,n+A+,Xr')  (9.52) 

where  /lra„(.)  represents  the  maximum  eigenvalue  of  (.) .  While  the  value  of  a,  M  cannot 

be  determined  a  priori,  the  filter  can  be  constructed  so  that  RM  is  chosen  such  that 
0<  At+I  <  1.  Similarly,  It  is  assumed  that  Fk  is  invertible  and  that  each  bjk  satisfies  the 
following  condition 

-VrT^^  ^VfT     for     j  =  \ n  (9.53) 


Ill 

with 

r.-ywf+ft)  (9.54) 

then  equation  (9.50)  is  satisfied.21  An  a  priori  choice  of  Rk+1  and  2(  should  be  chosen  to 
make  the  intervals  of  equations  (9.51)  and  (9.53)  as  large  as  possible.  In  the  limit 

J,I^J1-V1-A»*.-1  +  V1-A»+I]  =  [0.2]  (9.55) 

and 

^TJ-VrT-Vr^t—.H  (9.56) 

The  value  of  ajM  can  be  computed  by  running  the  filter  and  local  asymptotic  stability 
can  then  be  checked.     Therefore  with  Rltl  and  Qk  chosen  properly  and  a(  l+1  satisfying 
equation  (9.51)  the  local  asymptotic  stability  of  the  MSDREF  is  shown.  No  assumptions 
of  a  minimum  order  realization  for  the  estimator  are  made. 

It  is  obvious  that  equation  (9.55)  is  the  limiting  factor  in  this  approach  as  the 
bounds  are  severely  restricted.  It  should  be  noted,  however,  that  there  is  a  large  class  of 
nonlinear  physical  processes  where  the  measurement  process  is  linear,  therefore 
yk  =  Hxk .  This  makes  aiMi  =  Ip  and  equation  (9.55)  becomes  easily  satisfied.  Qt  can 
then  be  varied  over  all  possible  values  to  satisfy  equation  (9.56).  Therefore,  a  linear 
measurement  process  allows  the  local  asymptotic  stability  conditions  to  be  met  for  a 
much  wider  set  of  conditions  on  Rk  and  Qk . 


CHAPTER  10 
MSDREF  PROBLEM  SIMULATION  RESULTS 

There  are  cases  in  which  the  SDREF  formulation  has  distinct  advantages  over  the 
linearization  technique  used  in  the  EKE  Consider  the  system 

-     fol 


ol  (101) 


z  =  h{x)  =  \  +v  (10.2) 


where  x,(Q)  and  x,(0)  are  zero-mean  Gaussian-distributed  random  variables.  Let  m  and  n 
be  the  dimensions  of  the  measurement  and  state  vectors,  respectively  and  h(x)  =  H(x)x. 
Before  examining  the  filter  performance  a  proof  describing  the  nonlinear  observability  of 
the  system  is  examined.  Using  a  similar  methodology,22  and  using  the  complementary 
estimation  terms,  this  system  can  be  shown  to  be  weakly  observable  everywhere  except  at 
the  origin. 
Theorem: 
Given  the  system  equations: 


and  parameterizing  as 


*  =  /(*) 

z  =  k(x)  (103) 


/(*)  =  F(x)x 

Kx)  =  H{x)x  (104) 
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Let  m  >  n .  Assume  H(x)  has  rank  n  for  all  x.  Then  equation  (10.4)  is  observable  for  all 

x,  and  system  given  by  equation  (10.3)  is  weakly  observable  on  R" . 

Proof:  The  proof  is  given  in21  and  the  results  are  used  here  to  demonstrate  the 

observability  of  the  system.  If  the  sufficient  condition21 

rank[Ac(x)]  =  n  (10.5) 

is  met,  the  system  is  said  to  be  weakly  observable  (on  S)  for  all  (xeS).  Using  the 

recursive  formula  for  computing  Ac,23  A(.  was  found  to  be 


which  can  be  shown  to  have  rank=\  at  x,  =  x2  =  0 .  Therefore,  the  system  is  weakly 
observable  everywhere  except  the  origin.  Three  filters  will  now  be  used  to  investigate 
their  performance  when  confronted  with  this  system. 

The  EKF  uses  a  Taylor  series  expansion  to  linearize  the  measurement  matrix 
about  the  current  state  estimate  as 


H(x)  = 


dh(x) 
dx 


1       1 


(10.6) 


It  is  easily  seen  that  H(x)  loses  rank  whenever  i,  =  i2,  and  the  system  appears 
unobservable  to  the  EKF  algorithm.  There  is  no  solution  for  this  problem  in  the  EKF;  the 
SDREF,  however,  has  two  distinct  parameterizations: 


#,(*)  = 


1    1 

and     H2(x)  = 

'  1      1 

0    x, 

x2     0 

(10.7) 


which  can  be  combined  to  form  the  parameterized  state-dependent  coefficient 
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measurement  matrix  as 


H(x,a)- 


1  1 

ax2    (l-a)i, 


(10.8) 


In  this  form  the  value  of  a  can  be  chosen  such  that  loss  of  rank  is  avoided. 

This  section  examines  the  problem  with  the  MSDREF  and  EKF.  In  addition  to 
these  two  filters  a  second-order  Gaussian  filter  (SOGF)  was  also  derived  to  extend  the 
Taylor  series  expansion  of  h(x)  to  include  second  order  terms.  As  shown  in  the  results, 
this  had  very  little  effect. 

The  equations  used  for  each  filter  are  given  below 
EKF  (Continuous): 


x  =  f(x)  +  K[z-h(x)] 

P  =  F(x)P  +  PFT(x)  +  Q-PHT(x)RtH(x)P 

K=PHT(x)R  ' 


where 


F(x)  = 
H(x)  = 


df(x) 


dx 
dh(x) 


dx 


(10.9) 


(10.10) 


Second-Order  Gaussian  (Continuous/Discrete): 


x  =  f(x)+-d2(f,P) 

P  =  F(x)P+PFT(x)  +  Q 

ZM(+)  =  x^(-)  +  Kk[zk-ht{xt(-))-±d2(ht,Pt(-))] 

K=Pk(-)Htr(xt(-))[Ht(xk{-))Pk(-)HtT(xk(-))  +  Rk+Ak 
Pk(+)  =  lI-KkHt(xt(-))]Pk(~) 


(10.11) 
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F(x)  = 

H(x)  = 

I 


df(x) 


dx 
dh(x) 


dx 


where  Ak  =-E[d2(ht,x(-)xT(-))d1(hi,x(-)xT(-))  ] 

-^(>h,Pt(-))d2(hk,Pt(-))T 


(10.12) 


and    d^(h,P)  =  trace 


d2h 


<kp<hct 


MSDREF  (Continuous): 


x  =  f{x)  +  K[z-H{x)h(x)] 

P  =  F(x)P  +  PF1 (x)  +  Q-  PH7 \xiR^H(£)P 

K  =  PHT(Z)R-' 


(10.13) 


where  H(x)  and  F(x)  come  from  parameterized  matrices.  The  three  filters  were  coded 
and  the  problem  simulated  for  10  seconds  at  a  1 00-Hz-update  rate.  Each  filter  was  tuned 
to  give  the  best  performance  that  could  be  achieved.  Parameters  used  for  the  simulation 


runs  were 


V  =  E[V(T)VT(T)]  = 

0.1  0  " 
0     0.01 

Q  =  E[w(t)wt(t))  = 

"1.0  0 ' 
0     1.0 

P(0)  = 

"1.0     0 " 
0     1.0 

(10.14) 


When  given  initial  conditions 


x(0)  = 
*(0)  = 


(10.15) 
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the  EKF  and  SOGF  could  not  converge  to  the  correct  answer.  The  SDREF,  however,  was 

able  to  achieve  an  accurate  estimate  of  the  true  states  using  a  =  0.8.  This  is  shown  in 

Figures  55  and  56.  The  same  results  were  found  for  any  true  initial  states  where  the  EKF 

and  SOGF  were  started  with  *,(())  =  x,(0)  =  0,  which  is  the  logical  expected  mean  value 

for  a  zero-mean  random  variable.  This  caused  an  algorithmic  loss  of  observability,  and 

the  filters  converged  to  the  wrong  estimate.  An  example  can  be  constructed  where  the 

state  estimates  never  cross  the  algorithmically  unobservable  condition  of  x,  =  x2  and 

therefore  the  three  filters  are  able  to  converge  properly.  One  example  is 

*(0)  =  {o°3 

Figures  57  and  58  show  the  adequate  convergence  for  this  case.  However,  the 
convergence  problem  should  be  started  at  the  expected  value  of  the  state,  *(0)  =  {0    0}7 , 
and  achieve  convergence  when  the  state  estimates  cross  the  algorithmically  unobservable 
condition.  Only  the  SDREF  formulation  allows  this  to  be  accomplished. 

This  simple  academic  problem  illustrates  the  usefulness  of  the  SDREF  technique 
when  loss  of  algorithmic  observability  is  an  issue.  There  is  nothing  that  can  be  done  to 
prevent  this  in  the  EKF  or  SOGF;  however,  the  SDREF  state-dependent-coefficient 
parameterizations  can  be  adjusted  to  avoid  these  conditions. 
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Figure  55.  True  vs.  estimated  XI  state. 
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Figure  56.  True  vs.  estimated  X2  state. 
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Figure  57.  True  vs.  estimated  XI  state, 
non-zero  initial  conditions. 
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Figure  58.  True  vs.  estimated  X2  state, 
non-zero  initial  conditions. 


CHAPTER  1 1 
CRITICAL  ANALYSIS  OF  RESULTS 

The  original  purpose  of  this  dissertation  was  to  explore  a  new  nonlinear  estimation 
technique,  in  hopes  of  improving  upon  the  limitations  of  the  EKP.  Analysis  of  the  new 
SDRE  filtering  technique  demonstrated  an  advantage  over  linearization  techniques  when 
confronted  with  a  problem  in  which  loss  of  algorithmic  observability  is  encountered.  For 
the  other  two  problems  examined,  there  was  little  difference  in  the  performance  of  the 
SDREF  and  EKF  filters. 

The  first  evidence  of  this  occurred  with  the  comparison  of  the  probability  distribution 
functions  for  the  two  filters.  The  SDREF  and  EKF  pdf's  were  shown  to  be  almost 
identical,  indicating  that  although  the  filter  models  are  arrived  at  by,  for  problems 
involving  full  observability,  different  methods,  the  internal  representation  of  the  pdf  is 
similar.  This  shows  that  there  is  little  benefit  gained  by  parameterizing  the  nonlinearities 
versus  using  the  EKF  linearization  techniques.  This  was  true  even  when  the  filters  were 
exposed  to  extreme  nonlinear  conditions  for  which  the  EKF  might  lose  track.  Again  both 
filters  behaved  in  a  similar  manner. 

To  verify  that  the  use  of  linear  dynamics  with  nonlinear  measurements,  did  not  cause 
this  behavior,  the  pendulum  problem  was  developed.  This  allowed  examination  of  the 
filters  when  using  both  nonlinear  dynamics  and  measurements.    Again  examination  of 
the  pdf  shape  and  filter  performance  yielded  similar  results. 
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A  noticeable  shortcoming  of  the  SDREF  is  the  inability  to  use  an  initial  covariance 

matrix  estimate.  This  proved  to  be  a  limitation  when  the  filter  is  exposed  to  off- 
nominal  initial  conditions.  It  caused  an  initial  oscillation  in  the  state  estimates,  which 
eventually  damped  out.  This  performance,  however,  was  not  satisfactory,  so  an 
initialization  method  was  created.  By  assuming  that  the  value  of  Pk,  which  the  SDREF 
computed  is  Pk{+),  the  SDREF  can  be  initialized  with  an  initial  covariance  Pk(-)  to  be 
used  at  time-step  zero.  While  this  is  somewhat  of  an  ad-hoc  technique,  it  proved 
successful  in  removing  some  of  the  oscillatory  behavior.  The  oscillatory  behavior  is  due 
to  the  fact  that  the  SDREF  could  be  viewed  as  a  multi  model  adaptive  filter,  which 
switches  among  a  bank  of  steady  state  filters  based  on  the  state  estimate  x.  When  viewed 
in  this  way,  it  is  logical  to  expect  that  the  filter  has  settled  on  a  steady  initial  condition. 
When  this  is  not  the  case,  the  oscillatory  behavior  is  exhibited. 

The  need  for  a  positive  semi-definite  solution  of  the  Ricatti  equation,  also  proved  to 
be  limiting  factor  on  the  SDREF.  While  the  EKF  could  handle  the  bearings-only 
measurement  problem,  the  SDREF  required  the  addition  of  a  range  measurement  to 
satisfy  the  detectability  requirements.  Once  the  range  measurement  was  added,  the  filter 
performance  was  similar  to  that  of  the  EKF.  However,  the  ability  to  add  measurements  to 
the  problem  is  not  always  be  feasible. 

The  SDREF  does  offer  the  advantage  of  multiple  parameterizations  in  hopes  of 
improving  performance.  This  is  what  allowed  the  MSDREF  to  be  successful  in  working 
the  observability  problem.  There  is  no  method  to  deal  with  this  shortcoming  in  the  EKF. 
The  selection  of  a  good  parameterization  is  similar  to  the  artful  "tuning"  of  an  EKF. 
Tuning  an  EKF  requires  trial  and  error  tweaking  of  the  process  and  measurement  noise 
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until  the  desired  performance  is  reached.  This  would  be  a  similar  scenario  for  the  state- 
dependent-coefficient  parameterization  selection  in  the  SDREF. 

One  of  the  most  important  results  to  emerge  from  this  work  is  the  local  asymptotic 
stability  proof  for  the  MSDREF.  It  was  shown  that  this  proof  becomes  stronger  when  the 
system  has  linear  measurements,  as  the  conditions  for  local  stability  are  easily  satisfied 
for  any  choice  of  Rt  and  Qt .  For  systems  involving  nonlinear  measurements  the  bounds 
on  Rt  were  shown  to  be  somewhat  more  restrictive. 

While  the  new  SDREF  did  not  prove  to  be  a  substantial  improvement  over  the  EKF 
for  most  problems,  the  results  of  this  dissertation  proved  valuable  in  understanding  why. 
Through  the  use  of  pdf  comparisons,  sensitivity  trades,  observability  and  controllability 
analyses,  and  performance  studies,  the  SDREF  has  been  thoroughly  investigated.  The 
results  are  valuable  in  understanding  the  internal  operations  of  this  filter,  its  applicability 
to  a  real  world  guidance  problem,  and  the  promise  of  an  alternate  filtering  technique 
when  confronted  with  loss  of  algorithmic  observability. 
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